def transformFt2Global(ftlist): global listener # transform ft data to global frame (pos_trasform, ori_trasform) = lookupTransform('base_link', 'link_ft', listener) rotmat = tfm.quaternion_matrix(ori_trasform) ft_global = np.dot(rotmat, ftlist + [1.0]) return ft_global[0:3].tolist()
def transformFt2Global(ftlist): # transform ft data to global frame (pos_trasform, ori_trasform) = lookupTransform(global_frame_id, '/link_ft', lr) rotmat = tfm.quaternion_matrix(ori_trasform) ft_global = np.dot(rotmat, ftlist + [1.0]) return ft_global[0:3].tolist()
def push_rotate(objPose=[1.95,0.25,1.4,0,0,0,1], binNum=4, objId = 'crayola_64_ct', bin_contents = None, robotConfig = None, grasp_range_lim=0.11, fing_width=0.06, isExecute = True, withPause = True): #~ ***************************************************************** obj_dim=get_obj_dim(objId) obj_dim=np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width=0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.2) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion) shelf_pose_tfm=np.array(shelf_pose_tfm_list) shelf_pose_orient=shelf_pose_tfm[0:3,0:3] #~ print 'shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos=shelf_pose_tfm[0:3,3] #~ print 'shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0]) shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1]) shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7]) obj_pose_tfm=np.array(obj_pose_tfm_list) obj_pose_orient=obj_pose_tfm[0:3,0:3] obj_pose_pos=obj_pose_tfm[0:3,3] #~ print 'obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0]) #~ print 'obj_X' #~ print obj_X obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1]) #~ print 'obj_Y' #~ print obj_Y obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2]) #~ print 'obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z)) obj_pose_orient_norm=obj_pose_orient_norm.transpose() #~ print 'obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm) #~ print 'proj' #~ print proj_vecY max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir]>0: hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir] else: hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir] #~ print 'hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec)); angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180.0/np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm=obj_dim[hand_norm_dir] print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX=np.dot(shelf_X,obj_pose_orient_norm) max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir]>0: fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir] else: fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir] #~ print 'fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis=obj_dim[fing_axis_dir] print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm) max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ)) Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir] #~ print 'Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis=obj_dim[Zaxis_dir] hand_Y=np.cross(hand_norm_vec,fing_axis_vec) #~ ***************************************************************** #################### DECISION STRATEGIES ######################### bin_inner_cnstr=get_bin_inner_cnstr() proj_handNorm_ShelfX=np.dot(hand_norm_vec,shelf_X) right_push=False left_push=False # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis # # print 'proj_handNorm_ShelfX' # print proj_handNorm_ShelfX if obj_dim_along_hand_norm>obj_dim_along_fingAxis: #~ print 'proj_vecY' #~ print proj_vecY #~ print proj_vecY[hand_norm_dir] #~ #~ print 'proj_vecX' #~ print proj_vecX #~ print proj_vecX[fing_axis_dir] #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' #~ else: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' if proj_handNorm_ShelfX>0: #Do left push Ypush_mult=1 left_push=True print '[PushRotate] Left Push' else: #Do right push Ypush_mult=-1 print '[PushRotate] right Push' right_push=True push_offset=(obj_dim_along_hand_norm/2.0) #(Instead pf pushing on edge we will push 5 mm inside) else: #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' #~ else: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' if proj_handNorm_ShelfX>0: #Do right push Ypush_mult=-1 print '[PushRotate] right Push' right_push=True else: #Do left push Ypush_mult=1 print '[PushRotate] Left Push' left_push=True push_offset=(obj_dim_along_fingAxis/2.0) num_intm_points=5 push_x_intm=np.zeros(num_intm_points+1) push_y_intm=np.zeros(num_intm_points+1) backWall_shelf=bin_inner_cnstr[binNum][1] backWall_world = coordinateFrameTransform([0,backWall_shelf,0], 'shelf', 'map', listener) back_check=backWall_world.pose.position.x-obj_dim_along_hand_norm/2.0 binRightWall=bin_inner_cnstr[binNum][0] binLeftWall=bin_inner_cnstr[binNum][1] binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) if left_push: sideWall_check=binRightWall_world.pose.position.y+(fing_width/2.0)+(obj_dim_along_fingAxis) if right_push: sideWall_check=binLeftWall_world.pose.position.y-(fing_width/2.0)-(obj_dim_along_fingAxis) for i in range(num_intm_points+1): push_x_intm[i]=obj_pose_pos[0]+(push_offset)*np.sin(((90/num_intm_points)*i*np.pi)/180.0) if push_x_intm[i]>back_check: push_x_intm[i]=back_check push_y_intm[i]=obj_pose_pos[1]+(Ypush_mult*push_offset)*np.cos(((90/num_intm_points)*i*np.pi)/180.0) if left_push: if push_y_intm[i]<sideWall_check: push_y_intm[i]=sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' if right_push: if push_y_intm[i]>sideWall_check: push_y_intm[i]=sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' push_x_intm=np.array(push_x_intm) #~ print'push_x_intm' #~ print push_x_intm push_y_intm=np.array(push_y_intm) #******************************************************************* #Move the hand to the center of the bin and open the hand based on the height of the object bin_mid_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum) binFloorHeight_world = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener) bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener) #******************************************************************* tcp_Z_off=0.005 push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z push_z_intm=(push_tcp_Z)*np.ones(push_x_intm.size) push_series_pos=np.vstack((push_x_intm,push_y_intm,push_z_intm)) push_series_pos=push_series_pos.transpose() #~ print 'push_series_pos' #~ print push_series_pos #~ print push_series_pos[0,:] #~ print push_series_pos[-1,:] push_possible=True #****************************************************************** fing_push_pt=0.2*obj_dim_along_ZAxis+binFloorHeight_world.pose.position.z blade_tip_TCP_off=0.038 # dist between finger inner end and spatual edge push_hand_opening=2*(push_tcp_Z-blade_tip_TCP_off-fing_push_pt) if push_hand_opening>grasp_range_lim: push_hand_opening=grasp_range_lim # Spatula finger should not hit the lip of the bin while pushing lip_Z_shelf=bin_inner_cnstr[binNum][4] lip_Z_world=coordinateFrameTransform([0,0,lip_Z_shelf], 'shelf', 'map', listener) #~ lip_off=.025 #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off max_allowed_hand_opening_out=2*(push_tcp_Z-lip_Z_world.pose.position.z) max_allowed_hand_opening=max_allowed_hand_opening_out-0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface if push_hand_opening>max_allowed_hand_opening: print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin' push_hand_opening=max_allowed_hand_opening #~ print 'push_hand_opening' #~ print push_hand_opening #~ #hand should not hit the side walls #~ binRightWall,binLeftWall=find_shelf_walls(binNum) #~ #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) side_wall_clearance=0.0 print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y print '[PushRotate] right_hand_edge=', (push_y_intm[0]-fing_width-side_wall_clearance) print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y print '[PushRotate] left_hand_edge=', (push_y_intm[0]+fing_width+side_wall_clearance) if push_y_intm[0]-fing_width-side_wall_clearance<binRightWall_world.pose.position.y: print '[PushRotate] Hand will hit the right wall, Push rotate not possible' push_possible=False if push_y_intm[0]+fing_width+side_wall_clearance>binLeftWall_world.pose.position.y: print '[PushRotate] Hand will hit the left wall, push rotate not possible' push_possible=False #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient=deepcopy(spatula_down_orient) #~ ************************************************************* if push_possible==True: #Now we know where we wan to move TCP #Let's do robot motion planning now joint_topic = '/joint_states' # plan store plans = [] # set tcp (same as that set in generate dictionary) l1=0.0 l2=0.0 l3 = 0.47 tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized # broadcast frame attached to tcp pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z] gripperOri=[0, 0.7071, 0, 0.7071] pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushRotate] move to bin mouth in push-rotate code successful' #plan.visualize() plans.append(plan) # Plan 0 #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[PushRotate] move to bin mouth in push-rotate code fail' return False qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ moveGripper(0.1,100) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1=0.0 #0.035 push_l2=-Ypush_mult*(fing_width/2.0) # This should depend on the righ or left push conditions push_l3 = 0.47 push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) #~ q_initial = robotConfig # move just inside the bin with push orientation and open the hand suitable to push distFromShelf = -0.01 InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [InbinPt.pose.position.x, push_series_pos[0,1] , InbinPt.pose.position.z] #matching world_Y of first push pt print '[PushRotate] prepush_targetPosition=', prepush_targetPosition pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] move inside bin in push orient successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] move inside bin in push orient fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH ############### # Open the gripper to dim calculated for pushing print '[PushRotate] hand opening to' print push_hand_opening*1000.0 #~ moveGripper(push_hand_opening,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## #~ push_rotate_traj = Plan() #~ push_rotate_traj.q_traj = qf for i in range(0,push_x_intm.size): pushpt_pos=push_series_pos[i,:].transpose() pubFrame(br, pose=pushpt_pos.tolist()+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) #~ pdb.set_trace() # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient, # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb) planner = IK(q0 = q_initial, target_tip_pos = pushpt_pos, target_tip_ori = push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] point inside push traj successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] point inside push traj fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* #~ if isExecute: #~ pauseFunc(withPause) #~ push_rotate_traj.execute() #################### CLOSE THE GRIPPER GRASP ################# #~ moveGripper(0.0,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[numOfPlan].visualize() pauseFunc(withPause) plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[len(plans)-numOfPlan-1].visualizeBackward() pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() #******************************************************************* print '[PushRotate] push_successful' return (push_possible,False)
# get the marker pos from vicon vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg() rospy.sleep(0.2) #pause() try: vmpos = ( np.array(xyztolist(vmarkers.markers[-1].translation)) / 1000.0).tolist() except: print 'vicon pos is bad, not using this data' continue # get the marker pos from robot #(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener) (vicontrans, rot) = lookupTransform('/map', '/vicon_tip', listener) vmpos_robot = list(vicontrans) # test if the marker is tracked or not, skip print 'vicon pos %.4f %.4f %.4f' % tuple( vmpos), 'robot pos %.4f %.4f %.4f' % tuple(vmpos_robot) if norm(vmpos) < 1e-9: print 'vicon pos is bad, not using this data' continue xs = xs + [vmpos[0]] ys = ys + [vmpos[1]] zs = zs + [vmpos[2]] xt = xt + [vmpos_robot[0]] yt = yt + [vmpos_robot[1]] zt = zt + [vmpos_robot[2]]
def main(argv): # prepare the proxy, listener global listener global vizpub rospy.init_node('contour_follow', anonymous=True) listener = tf.TransformListener() vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10) br = TransformBroadcaster() setSpeed(tcp=100, ori=30) setZone(0) # set the parameters limit = 10000 # number of data points to be collected ori = [0, 0.7071, 0.7071, 0] threshold = 0.3 # the threshold force for contact, need to be tuned z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218 probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745 step_size = 0.0002 obj_des_wrt_vicon = [0,0,-(9.40/2/1000+14.15/2/1000),0,0,0,1] # visualize the block vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) # 0. move to startPos start_pos = [0.3, 0.06, z + 0.05] setCart(start_pos,ori) start_pos = [0.3, 0.06, z] setCart(start_pos,ori) curr_pos = start_pos # 0.1 zero the ft reading rospy.sleep(1) setZero() rospy.sleep(3) # 1. move in -y direction till contact -> normal setSpeed(tcp=30, ori=30) direc = np.array([0, -step_size, 0]) normal = [0,0,0] while True: curr_pos = np.array(curr_pos) + direc setCart(curr_pos, ori) # let the ft reads the force in static, not while pushing rospy.sleep(0.1) ft = ftmsg2list(ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg()) print '[ft explore]', ft # get box pose from vicon (box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener) # correct box_pose box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon))) print 'box_pose', box_pose_des_global # If in contact, break if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) break #pause() # 2. use the normal to move along the block setSpeed(tcp=20, ori=30) index = 0 contact_pts = [] contact_nms = [] all_contact = [] while True: # 2.1 move direc = np.dot(tfm.euler_matrix(0,0,2) , normal.tolist() + [1])[0:3] curr_pos = np.array(curr_pos) + direc * step_size setCart(curr_pos, ori) # let the ft reads the force in static, not while pushing rospy.sleep(0.1) ft = getAveragedFT() print index #, '[ft explore]', ft # get box pose from vicon (box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener) # correct box_pose box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon))) #box_pose_des_global = list(box_pos) + list(box_quat) #print 'box_pose', box_pose_des_global vizBlock(obj_des_wrt_vicon) br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7], rospy.Time.now(), "SteelBlockDesired", "map") #print 'box_pos', box_pos, 'box_quat', box_quat if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) contact_nms.append(normal.tolist()) contact_pt = curr_pos - normal * probe_radis contact_pts.append(contact_pt.tolist()) contact_pt[2] = 0.01 vizPoint(contact_pt.tolist()) vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist()) # caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object. all_contact.append(contact_pt.tolist()[0:2] + [0] + (-normal).tolist()[0:2] + [0] + box_pose_des_global[0:3] + box_pose_des_global[6:7] + box_pose_des_global[3:6] + curr_pos.tolist()) index += 1 if len(contact_pts) > limit: break if len(contact_pts) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back move_away_size = 0.01 overshoot_size = 0 #0.0005 setSpeed(tcp=5, ori=30) setCart(curr_pos + normal * move_away_size, ori) rospy.sleep(1) print 'bad ft:', getAveragedFT() setZero() rospy.sleep(3) setCart(curr_pos - normal * overshoot_size, ori) setSpeed(tcp=20, ori=30) #all_contact(1:2,:); % x,y of contact position #all_contact(4:5,:); % x,y contact normal #all_contact(7:9,:); % box x,y #all_contact(10:13,:); % box quaternion #all_contact(14:16,:); % pusher position # save contact_nm and contact_pt as json file with open(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.json', 'w') as outfile: json.dump({'contact_pts': contact_pts, 'contact_nms': contact_nms}, outfile) # save all_contact as mat file sio.savemat(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.mat', {'all_contact': all_contact}) setSpeed(tcp=100, ori=30) # 3. move back to startPos start_pos = [0.3, 0.06, z + 0.05] setCart(start_pos,ori)
def _detectOneObject(obj_id, bin_num, kinect_num): global _detect_one_object_srv global br print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num # filter the point cloud pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num) if pc is None: return (None, None) # string[] model_names # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names' # ObjectConstraint[] constraints # These apply to all objects # --- # SceneHypothesis[] detections # 2. prepare constraints bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.2 left ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.3 back ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.4 front ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.5 floor ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.6 top ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]-floor_thick/2], trans,rot, pc.header.frame_id), tol, bin_num) ) # string model_name # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # ObjectConstraint[] constraints # geometry_msgs/Pose true_pose # for testing # --- # # ObjectHypothesis[] detections with Timer('detect_one_object'): # detect using capsen service_name = '/detection_service/detect_one_object' req = DetectOneObjectRequest() req.model_name = obj_id req.cloud = pc req.constraints = ccnstr req.foreground_mask = foreground_mask #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] print 'Waiting for service up: ', service_name rospy.wait_for_service(service_name) try: print 'Calling service:', service_name ret = _detect_one_object_srv(req) # ret.detections is a list of capsen_vision/ObjectHypothesis # string name # geometry_msgs/Pose pose # float32 score # float32[] score_components if len(ret.detections)>0: print len(ret.detections), 'ObjectHypothesis returned, max score', ret.detections[0].score for i in range(len(ret.detections)): poselist_capsen_world = poseTransform(pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) # transform to our desired pose # check whether inside bin poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener) if inside_bin(poselist_shelf[0:3], bin_num): #pubFrame(br, poselist_world, 'obj', 'map') return (poselist_world, ret.detections[i].score) else: print 'reject hypo', i, 'because it is outside the target bin' print 'No ObjectHypothesis satisfy hard bin constraint' return (None, None) else: print 'No ObjectHypothesis returned' return (None, None) except: print 'Calling service:', service_name, 'failed' print 'encounters errors:', traceback.format_exc() return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num): # return pose, retScore global _detect_objects_srv global br global _tflistener print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num # 1. filter the point cloud pc, foreground_mask = get_filtered_pointcloud(obj_ids, bin_num, kinect_num) # need to pass in list if pc is None or foreground_mask is None: return (None, None) # 2. prepare constraints bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.2 left ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.3 back ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.4 front ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.5 floor ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.6 top ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) ) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]+floor_thick*2], trans,rot, pc.header.frame_id), tol, bin_num) ) #visualizeConstraint(ccnstr[6], pc.header.frame_id) #pause() # 3. detect using capsen with Timer('detect_objects'): service_name = '/detection_service/detect_objects' req = DetectObjectsRequest() req.model_names = obj_ids req.constraints = ccnstr req.cloud = pc req.foreground_mask = foreground_mask sum_pt = sum(foreground_mask) if allFalse(foreground_mask, sum_pt): return (None, None) foreground_mask = subsample(foreground_mask, sum_pt) # outputfile = '/tmp/foreground_mask' # with open(outputfile, 'w') as outfile: # json.dump(foreground_mask, outfile) # pause() #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack req.find_exact_object_list = True print '\tWaiting for service up: ', service_name rospy.wait_for_service(service_name) #pdb.set_trace() try: print '\tCalling service:', service_name ret = _detect_objects_srv(req) # ret.detections is a list of capsen_vision/SceneHypothesis # [capsen_vision/SceneHypothesis]: # std_msgs/Header header # uint32 seq # time stamp # string frame_id # capsen_vision/ObjectHypothesis[] objects # string name # geometry_msgs/Pose pose # geometry_msgs/Point position # float64 x # float64 y # float64 z # geometry_msgs/Quaternion orientation # float64 x # float64 y # float64 z # float64 w # float32 score # float32[] score_components # float32[2] errors # float32 score if len(ret.detections)>0: print '\t', len(ret.detections), 'SceneHypothesis returned, max score', ret.detections[0].score #print ret.detections for i in range(len(ret.detections)): scene = ret.detections[i] nobj = len(scene.objects) scene_desired = transformObjectsFromCapsenToDesiredFrame(scene, pc.header.frame_id) if allObjectsInsideBin(scene_desired, bin_num): return (scene_desired.objects, scene_desired.score) #else: #print 'reject scene hypo', i, 'because one object of it is outside the target bin' print '\tNo SceneHypothesis satisfy hard bin constraint' return (None, None) else: print '\tNo SceneHypothesis returned' return (None, None) except: print '\tCalling service:', service_name, 'failed' print '\tencounters errors:', traceback.format_exc() return (None, None)
def run_explore(filename): # 0. check if collected or not dir_base = data_base + "/contour_following/" jsonfilename = dir_base + '/%s.json' % filename matfilename = dir_base + '/%s.mat' % filename if os.path.isfile(jsonfilename): print "skip", filename return # 1. move to startPos setSpeed(tcp=global_vel) setZone(0) start_pos = [center_world[0] + explore_radius, center_world[1]] + [zup] setCart(start_pos) # 2. Rough probing scan_contact_pts = [] # 2.1 populate start poses for small probing in opposite direction start_configs = [] jmax = (nstart + 1) // 2 kmax = 2 if nstart > 1 else 1 dth = 2 * np.pi / nstart for j in xrange(jmax): for k in xrange(kmax): i = j + k * (nstart // 2) start_pos = [ center_world[0] + explore_radius * np.cos(i * dth), center_world[1] + explore_radius * np.sin(i * dth) ] direc = [-np.cos(i * dth), -np.sin(i * dth), 0] start_configs.append([start_pos, direc]) print 'direc', direc # 2.2 move toward center till contact, and record contact points for i, (start_pos, direc) in enumerate(reversed(start_configs)): setZero() wait_for_ft_calib() setCart(start_pos + [zup]) setCart(start_pos + [z]) curr_pos = start_pos + [z] cnt = 0 while True: curr_pos = np.array(curr_pos) + np.array(direc) * step_size setCart(curr_pos) # let the ft reads the force in static, not while pushing rospy.sleep(sleepForFT) ft = getFTmsglist() print '[ft explore]', ft # get box pose from vicon (box_pos, box_quat) = lookupTransform(global_frame_id, obj_frame_id, lr) box_pose_des_global = list(box_pos) + list(box_quat) print 'obj_pose', box_pose_des_global # If in contact, break if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) contact_pt = curr_pos - normal * probe_radius scan_contact_pts.append(contact_pt.tolist()) if i < len( start_configs ) - 1: # if the last one, stay in contact and do exploration from there. setCart([curr_pos[0], curr_pos[1], zup]) break #if too far and no contact break. if cnt > explore_radius * 2 / step_size: break if len(scan_contact_pts) == 0: print "Error: Cannot touch the object" return # 3. Contour following, use the normal to move along the block setSpeed(tcp=global_slow_vel) index = 0 contact_pts = [] contact_nms = [] all_contact = [] while True: # 3.1 move direc = np.dot(tfm.euler_matrix(0, 0, 2), normal.tolist() + [1])[0:3] curr_pos = np.array(curr_pos) + direc * step_size setCart(curr_pos) # 3.2 let the ft reads the force in static, not while pushing rospy.sleep(sleepForFT) ft = getAveragedFT() if index % 50 == 0: #print index #, '[ft explore]', ft print index else: sys.stdout.write('.') sys.stdout.flush() # get box pose from vicon (box_pos, box_quat) = lookupTransform(global_frame_id, obj_frame_id, lr) box_pose_des_global = list(box_pos) + list(box_quat) #print 'box_pose', box_pose_des_global # 3.3 visualize it. vizBlock(box_pose_des_global, obj_mesh, global_frame_id) # 3.4 record the data if in contact if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) contact_nms.append(normal.tolist()) contact_pt = curr_pos - normal * probe_radius contact_pts.append(contact_pt.tolist()) vizPoint(contact_pt.tolist()) vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist()) # caution: matlab uses the other quaternion order: w x y z. # Also the normal should point toward the object. all_contact.append(contact_pt.tolist()[0:2] + [0] + (-normal).tolist()[0:2] + [0] + box_pose_des_global[0:3] + box_pose_des_global[6:7] + box_pose_des_global[3:6] + curr_pos.tolist()) index += 1 # 3.5 break if we collect enough if len(contact_pts) > limit: break # 3.6 periodically zero the ft if len( contact_pts ) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back move_away_size = 0.01 overshoot_size = 0 #0.0005 setSpeed(tcp=global_super_slow_vel) setCart(curr_pos + normal * move_away_size) rospy.sleep(1) print 'offset ft:', getAveragedFT() setZero() wait_for_ft_calib() setCart(curr_pos - normal * overshoot_size) setSpeed(tcp=global_slow_vel) # 4.1 save contact_nm and contact_pt as json file helper.make_sure_path_exists(dir_base) with open(jsonfilename, 'w') as outfile: json.dump( { 'all_contact': all_contact, 'scan_contact_pts': scan_contact_pts, 'isreal': True, '__title__': colname, "surface_id": surface_id, "shape_id": shape_id, "probe_id": probe_id, "muc": muc, "probe_radius": probe_radius, "offset": offset }, outfile, sort_keys=True, indent=1) # 4.2 save all_contact as mat file sio.savemat(matfilename, { 'all_contact': all_contact, 'scan_contact_pts': scan_contact_pts }) # 5. move back to startPos setSpeed(tcp=global_vel) start_pos = [curr_pos[0], curr_pos[1]] + [zup] setCart(start_pos) recover(obj_slot, True)
def push_rotate(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1], binNum=4, objId='crayola_64_ct', bin_contents=None, robotConfig=None, grasp_range_lim=0.11, fing_width=0.06, isExecute=True, withPause=True): #~ ***************************************************************** obj_dim = get_obj_dim(objId) obj_dim = np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width = 0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.2) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion) shelf_pose_tfm = np.array(shelf_pose_tfm_list) shelf_pose_orient = shelf_pose_tfm[0:3, 0:3] #~ print 'shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos = shelf_pose_tfm[0:3, 3] #~ print 'shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0]) shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1]) shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7]) obj_pose_tfm = np.array(obj_pose_tfm_list) obj_pose_orient = obj_pose_tfm[0:3, 0:3] obj_pose_pos = obj_pose_tfm[0:3, 3] #~ print 'obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0]) #~ print 'obj_X' #~ print obj_X obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1]) #~ print 'obj_Y' #~ print obj_Y obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2]) #~ print 'obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z)) obj_pose_orient_norm = obj_pose_orient_norm.transpose() #~ print 'obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm) #~ print 'proj' #~ print proj_vecY max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax( np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir] > 0: hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir] else: hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir] #~ print 'hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) * la.norm(hand_norm_vec)) angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180.0 / np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm = obj_dim[hand_norm_dir] print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX = np.dot(shelf_X, obj_pose_orient_norm) max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax( np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir] > 0: fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir] else: fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir] #~ print 'fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis = obj_dim[fing_axis_dir] print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm) max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax( np.fabs(proj_vecZ)) Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir] #~ print 'Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis = obj_dim[Zaxis_dir] hand_Y = np.cross(hand_norm_vec, fing_axis_vec) #~ ***************************************************************** #################### DECISION STRATEGIES ######################### bin_inner_cnstr = get_bin_inner_cnstr() proj_handNorm_ShelfX = np.dot(hand_norm_vec, shelf_X) right_push = False left_push = False # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis # # print 'proj_handNorm_ShelfX' # print proj_handNorm_ShelfX if obj_dim_along_hand_norm > obj_dim_along_fingAxis: #~ print 'proj_vecY' #~ print proj_vecY #~ print proj_vecY[hand_norm_dir] #~ #~ print 'proj_vecX' #~ print proj_vecX #~ print proj_vecX[fing_axis_dir] #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' #~ else: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' if proj_handNorm_ShelfX > 0: #Do left push Ypush_mult = 1 left_push = True print '[PushRotate] Left Push' else: #Do right push Ypush_mult = -1 print '[PushRotate] right Push' right_push = True push_offset = ( obj_dim_along_hand_norm / 2.0 ) #(Instead pf pushing on edge we will push 5 mm inside) else: #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' #~ else: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' if proj_handNorm_ShelfX > 0: #Do right push Ypush_mult = -1 print '[PushRotate] right Push' right_push = True else: #Do left push Ypush_mult = 1 print '[PushRotate] Left Push' left_push = True push_offset = (obj_dim_along_fingAxis / 2.0) num_intm_points = 5 push_x_intm = np.zeros(num_intm_points + 1) push_y_intm = np.zeros(num_intm_points + 1) backWall_shelf = bin_inner_cnstr[binNum][1] backWall_world = coordinateFrameTransform([0, backWall_shelf, 0], 'shelf', 'map', listener) back_check = backWall_world.pose.position.x - obj_dim_along_hand_norm / 2.0 binRightWall = bin_inner_cnstr[binNum][0] binLeftWall = bin_inner_cnstr[binNum][1] binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0], 'shelf', 'map', listener) binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf', 'map', listener) if left_push: sideWall_check = binRightWall_world.pose.position.y + ( fing_width / 2.0) + (obj_dim_along_fingAxis) if right_push: sideWall_check = binLeftWall_world.pose.position.y - ( fing_width / 2.0) - (obj_dim_along_fingAxis) for i in range(num_intm_points + 1): push_x_intm[i] = obj_pose_pos[0] + (push_offset) * np.sin( ((90 / num_intm_points) * i * np.pi) / 180.0) if push_x_intm[i] > back_check: push_x_intm[i] = back_check push_y_intm[i] = obj_pose_pos[1] + (Ypush_mult * push_offset) * np.cos( ((90 / num_intm_points) * i * np.pi) / 180.0) if left_push: if push_y_intm[i] < sideWall_check: push_y_intm[i] = sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' if right_push: if push_y_intm[i] > sideWall_check: push_y_intm[i] = sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' push_x_intm = np.array(push_x_intm) #~ print'push_x_intm' #~ print push_x_intm push_y_intm = np.array(push_y_intm) #******************************************************************* #Move the hand to the center of the bin and open the hand based on the height of the object bin_mid_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum) binFloorHeight_world = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf', 'map', listener) bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener) #******************************************************************* tcp_Z_off = 0.005 push_tcp_Z_shelfFrame = ( (bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) / 2.0) - tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform( [0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z push_z_intm = (push_tcp_Z) * np.ones(push_x_intm.size) push_series_pos = np.vstack((push_x_intm, push_y_intm, push_z_intm)) push_series_pos = push_series_pos.transpose() #~ print 'push_series_pos' #~ print push_series_pos #~ print push_series_pos[0,:] #~ print push_series_pos[-1,:] push_possible = True #****************************************************************** fing_push_pt = 0.2 * obj_dim_along_ZAxis + binFloorHeight_world.pose.position.z blade_tip_TCP_off = 0.038 # dist between finger inner end and spatual edge push_hand_opening = 2 * (push_tcp_Z - blade_tip_TCP_off - fing_push_pt) if push_hand_opening > grasp_range_lim: push_hand_opening = grasp_range_lim # Spatula finger should not hit the lip of the bin while pushing lip_Z_shelf = bin_inner_cnstr[binNum][4] lip_Z_world = coordinateFrameTransform([0, 0, lip_Z_shelf], 'shelf', 'map', listener) #~ lip_off=.025 #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off max_allowed_hand_opening_out = 2 * (push_tcp_Z - lip_Z_world.pose.position.z) max_allowed_hand_opening = max_allowed_hand_opening_out - 0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface if push_hand_opening > max_allowed_hand_opening: print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin' push_hand_opening = max_allowed_hand_opening #~ print 'push_hand_opening' #~ print push_hand_opening #~ #hand should not hit the side walls #~ binRightWall,binLeftWall=find_shelf_walls(binNum) #~ #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) side_wall_clearance = 0.0 print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y print '[PushRotate] right_hand_edge=', (push_y_intm[0] - fing_width - side_wall_clearance) print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y print '[PushRotate] left_hand_edge=', (push_y_intm[0] + fing_width + side_wall_clearance) if push_y_intm[ 0] - fing_width - side_wall_clearance < binRightWall_world.pose.position.y: print '[PushRotate] Hand will hit the right wall, Push rotate not possible' push_possible = False if push_y_intm[ 0] + fing_width + side_wall_clearance > binLeftWall_world.pose.position.y: print '[PushRotate] Hand will hit the left wall, push rotate not possible' push_possible = False #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient = deepcopy(spatula_down_orient) #~ ************************************************************* if push_possible == True: #Now we know where we wan to move TCP #Let's do robot motion planning now joint_topic = '/joint_states' # plan store plans = [] # set tcp (same as that set in generate dictionary) l1 = 0.0 l2 = 0.0 l3 = 0.47 tip_hand_transform = [ l1, l2, l3, 0, 0, 0 ] # to be updated when we have a hand design finalized # broadcast frame attached to tcp pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [ mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z ] gripperOri = [0, 0.7071, 0, 0.7071] pubFrame(br, pose=targetPosition + gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushRotate] move to bin mouth in push-rotate code successful' #plan.visualize() plans.append(plan) # Plan 0 #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[PushRotate] move to bin mouth in push-rotate code fail' return False qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ moveGripper(0.1,100) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1 = 0.0 #0.035 push_l2 = -Ypush_mult * ( fing_width / 2.0 ) # This should depend on the righ or left push conditions push_l3 = 0.47 push_tip_hand_transform = [ l1, l2, l3, 0, 0, 0 ] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) #~ q_initial = robotConfig # move just inside the bin with push orientation and open the hand suitable to push distFromShelf = -0.01 InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [ InbinPt.pose.position.x, push_series_pos[0, 1], InbinPt.pose.position.z ] #matching world_Y of first push pt print '[PushRotate] prepush_targetPosition=', prepush_targetPosition pubFrame(br, pose=prepush_targetPosition + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0=q_initial, target_tip_pos=prepush_targetPosition, target_tip_ori=push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] move inside bin in push orient successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] move inside bin in push orient fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH ############### # Open the gripper to dim calculated for pushing print '[PushRotate] hand opening to' print push_hand_opening * 1000.0 #~ moveGripper(push_hand_opening,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## #~ push_rotate_traj = Plan() #~ push_rotate_traj.q_traj = qf for i in range(0, push_x_intm.size): pushpt_pos = push_series_pos[i, :].transpose() pubFrame(br, pose=pushpt_pos.tolist() + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) #~ pdb.set_trace() # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient, # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb) planner = IK(q0=q_initial, target_tip_pos=pushpt_pos, target_tip_ori=push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] point inside push traj successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] point inside push traj fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* #~ if isExecute: #~ pauseFunc(withPause) #~ push_rotate_traj.execute() #################### CLOSE THE GRIPPER GRASP ################# #~ moveGripper(0.0,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[numOfPlan].visualize() pauseFunc(withPause) plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[len(plans) - numOfPlan - 1].visualizeBackward() pauseFunc(withPause) plans[len(plans) - numOfPlan - 1].executeBackward() #******************************************************************* print '[PushRotate] push_successful' return (push_possible, False)
def main(argv): # prepare the proxy, listener global listener global vizpub rospy.init_node('contour_follow', anonymous=True) listener = tf.TransformListener() vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10) br = TransformBroadcaster() setSpeed(tcp=100, ori=30) setZone(0) # set the parameters limit = 10000 # number of data points to be collected ori = [0, 0.7071, 0.7071, 0] threshold = 0.3 # the threshold force for contact, need to be tuned z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218 probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745 step_size = 0.0002 obj_des_wrt_vicon = [ 0, 0, -(9.40 / 2 / 1000 + 14.15 / 2 / 1000), 0, 0, 0, 1 ] # visualize the block vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) vizBlock(obj_des_wrt_vicon) rospy.sleep(0.1) # 0. move to startPos start_pos = [0.3, 0.06, z + 0.05] setCart(start_pos, ori) start_pos = [0.3, 0.06, z] setCart(start_pos, ori) curr_pos = start_pos # 0.1 zero the ft reading rospy.sleep(1) setZero() rospy.sleep(3) # 1. move in -y direction till contact -> normal setSpeed(tcp=30, ori=30) direc = np.array([0, -step_size, 0]) normal = [0, 0, 0] while True: curr_pos = np.array(curr_pos) + direc setCart(curr_pos, ori) # let the ft reads the force in static, not while pushing rospy.sleep(0.1) ft = ftmsg2list( ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg()) print '[ft explore]', ft # get box pose from vicon (box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener) # correct box_pose box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon))) print 'box_pose', box_pose_des_global # If in contact, break if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) break #pause() # 2. use the normal to move along the block setSpeed(tcp=20, ori=30) index = 0 contact_pts = [] contact_nms = [] all_contact = [] while True: # 2.1 move direc = np.dot(tfm.euler_matrix(0, 0, 2), normal.tolist() + [1])[0:3] curr_pos = np.array(curr_pos) + direc * step_size setCart(curr_pos, ori) # let the ft reads the force in static, not while pushing rospy.sleep(0.1) ft = getAveragedFT() print index #, '[ft explore]', ft # get box pose from vicon (box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener) # correct box_pose box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon))) #box_pose_des_global = list(box_pos) + list(box_quat) #print 'box_pose', box_pose_des_global vizBlock(obj_des_wrt_vicon) br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7], rospy.Time.now(), "SteelBlockDesired", "map") #print 'box_pos', box_pos, 'box_quat', box_quat if norm(ft[0:2]) > threshold: # transform ft data to global frame ft_global = transformFt2Global(ft) ft_global[2] = 0 # we don't want noise from z normal = ft_global[0:3] / norm(ft_global) contact_nms.append(normal.tolist()) contact_pt = curr_pos - normal * probe_radis contact_pts.append(contact_pt.tolist()) contact_pt[2] = 0.01 vizPoint(contact_pt.tolist()) vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist()) # caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object. all_contact.append(contact_pt.tolist()[0:2] + [0] + (-normal).tolist()[0:2] + [0] + box_pose_des_global[0:3] + box_pose_des_global[6:7] + box_pose_des_global[3:6] + curr_pos.tolist()) index += 1 if len(contact_pts) > limit: break if len( contact_pts ) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back move_away_size = 0.01 overshoot_size = 0 #0.0005 setSpeed(tcp=5, ori=30) setCart(curr_pos + normal * move_away_size, ori) rospy.sleep(1) print 'bad ft:', getAveragedFT() setZero() rospy.sleep(3) setCart(curr_pos - normal * overshoot_size, ori) setSpeed(tcp=20, ori=30) #all_contact(1:2,:); % x,y of contact position #all_contact(4:5,:); % x,y contact normal #all_contact(7:9,:); % box x,y #all_contact(10:13,:); % box quaternion #all_contact(14:16,:); % pusher position # save contact_nm and contact_pt as json file with open(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.json', 'w') as outfile: json.dump({ 'contact_pts': contact_pts, 'contact_nms': contact_nms }, outfile) # save all_contact as mat file sio.savemat(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.mat', {'all_contact': all_contact}) setSpeed(tcp=100, ori=30) # 3. move back to startPos start_pos = [0.3, 0.06, z + 0.05] setCart(start_pos, ori)
def _detectOneObject(obj_id, bin_num, kinect_num): global _detect_one_object_srv global br print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num # filter the point cloud pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num) if pc is None: return (None, None) # string[] model_names # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names' # ObjectConstraint[] constraints # These apply to all objects # --- # SceneHypothesis[] detections # 2. prepare constraints bin_cnstr = get_bin_cnstr( )[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.2 left ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.3 back ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.4 front ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.5 floor ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.6 top ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint( ObjectConstraint.SUPPORTING_PLANE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] - floor_thick / 2], trans, rot, pc.header.frame_id), tol, bin_num)) # string model_name # sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480) # bool[] foreground_mask # ObjectConstraint[] constraints # geometry_msgs/Pose true_pose # for testing # --- # # ObjectHypothesis[] detections with Timer('detect_one_object'): # detect using capsen service_name = '/detection_service/detect_one_object' req = DetectOneObjectRequest() req.model_name = obj_id req.cloud = pc req.constraints = ccnstr req.foreground_mask = foreground_mask #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] print 'Waiting for service up: ', service_name rospy.wait_for_service(service_name) try: print 'Calling service:', service_name ret = _detect_one_object_srv(req) # ret.detections is a list of capsen_vision/ObjectHypothesis # string name # geometry_msgs/Pose pose # float32 score # float32[] score_components if len(ret.detections) > 0: print len( ret.detections ), 'ObjectHypothesis returned, max score', ret.detections[ 0].score for i in range(len(ret.detections)): poselist_capsen_world = poseTransform( pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw poselist_world = transformBack( cap_T_our, poselist_capsen_world) # transform to our desired pose # check whether inside bin poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener) if inside_bin(poselist_shelf[0:3], bin_num): #pubFrame(br, poselist_world, 'obj', 'map') return (poselist_world, ret.detections[i].score) else: print 'reject hypo', i, 'because it is outside the target bin' print 'No ObjectHypothesis satisfy hard bin constraint' return (None, None) else: print 'No ObjectHypothesis returned' return (None, None) except: print 'Calling service:', service_name, 'failed' print 'encounters errors:', traceback.format_exc() return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num): # return pose, retScore global _detect_objects_srv global br global _tflistener print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num # 1. filter the point cloud pc, foreground_mask = get_filtered_pointcloud( obj_ids, bin_num, kinect_num) # need to pass in list if pc is None or foreground_mask is None: return (None, None) # 2. prepare constraints bin_cnstr = get_bin_cnstr( )[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top ccnstr = [] tol = 0.9 # larger is more strict (trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener) # 2.1 right ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.2 left ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.3 back ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.4 front ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.5 floor ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.6 top ccnstr.append( createCapsenConstraint( ObjectConstraint.HALF_SPACE, transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot, pc.header.frame_id), tol, bin_num)) # 2.7 on floor floor_thick = 0.03 ccnstr.append( createCapsenConstraint( ObjectConstraint.SUPPORTING_PLANE, transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] + floor_thick * 2], trans, rot, pc.header.frame_id), tol, bin_num)) #visualizeConstraint(ccnstr[6], pc.header.frame_id) #pause() # 3. detect using capsen with Timer('detect_objects'): service_name = '/detection_service/detect_objects' req = DetectObjectsRequest() req.model_names = obj_ids req.constraints = ccnstr req.cloud = pc req.foreground_mask = foreground_mask sum_pt = sum(foreground_mask) if allFalse(foreground_mask, sum_pt): return (None, None) foreground_mask = subsample(foreground_mask, sum_pt) # outputfile = '/tmp/foreground_mask' # with open(outputfile, 'w') as outfile: # json.dump(foreground_mask, outfile) # pause() #req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack req.find_exact_object_list = True print '\tWaiting for service up: ', service_name rospy.wait_for_service(service_name) #pdb.set_trace() try: print '\tCalling service:', service_name ret = _detect_objects_srv(req) # ret.detections is a list of capsen_vision/SceneHypothesis # [capsen_vision/SceneHypothesis]: # std_msgs/Header header # uint32 seq # time stamp # string frame_id # capsen_vision/ObjectHypothesis[] objects # string name # geometry_msgs/Pose pose # geometry_msgs/Point position # float64 x # float64 y # float64 z # geometry_msgs/Quaternion orientation # float64 x # float64 y # float64 z # float64 w # float32 score # float32[] score_components # float32[2] errors # float32 score if len(ret.detections) > 0: print '\t', len( ret.detections ), 'SceneHypothesis returned, max score', ret.detections[ 0].score #print ret.detections for i in range(len(ret.detections)): scene = ret.detections[i] nobj = len(scene.objects) scene_desired = transformObjectsFromCapsenToDesiredFrame( scene, pc.header.frame_id) if allObjectsInsideBin(scene_desired, bin_num): return (scene_desired.objects, scene_desired.score) #else: #print 'reject scene hypo', i, 'because one object of it is outside the target bin' print '\tNo SceneHypothesis satisfy hard bin constraint' return (None, None) else: print '\tNo SceneHypothesis returned' return (None, None) except: print '\tCalling service:', service_name, 'failed' print '\tencounters errors:', traceback.format_exc() return (None, None)
setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th) # get the marker pos from vicon vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg() rospy.sleep(0.2) #pause() try: vmpos = (np.array(xyztolist(vmarkers.markers[-1].translation)) / 1000.0).tolist() except: print 'vicon pos is bad, not using this data' continue # get the marker pos from robot #(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener) (vicontrans,rot) = lookupTransform('/map','/vicon_tip', listener) vmpos_robot = list(vicontrans) # test if the marker is tracked or not, skip print 'vicon pos %.4f %.4f %.4f' % tuple(vmpos), 'robot pos %.4f %.4f %.4f' % tuple(vmpos_robot) if norm(vmpos) < 1e-9: print 'vicon pos is bad, not using this data' continue xs = xs + [vmpos[0]] ys = ys + [vmpos[1]] zs = zs + [vmpos[2]] xt = xt + [vmpos_robot[0]] yt = yt + [vmpos_robot[1]] zt = zt + [vmpos_robot[2]] print 'data length', len(xs)
setAcc(acc=globalacc, deacc=globalacc) for x in np.linspace(limits[0], limits[1], nseg[0]): for y in np.linspace(limits[2], limits[3], nseg[1]): for z in np.linspace(limits[4], limits[5], nseg[2]): setCart([x, y, z], ori) js = getJoint() #raw_input('pause') # to see where is bad for th in np.linspace(-180, 180, nrotate): setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th) # get the marker pos from vicon rospy.sleep(0.2) (vmarkerstrans, rot) = lookupTransform( '/viconworld', '/vicon/CalibViconPlate/CalibViconPlate', listener) #pause() try: vmpos = list(vmarkerstrans) except: print 'vicon pos is bad, not using this data' continue # get the marker pos from robot #(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener) (vicontrans, rot) = lookupTransform('/map', '/vicon_tip', listener) vmpos_robot = list(vicontrans) # test if the marker is tracked or not, skip