def go_up_arc(): gripper.open() ik.helper.move_cart(dz=.3) goToHome.goToARC()
def run_experiments(self): ####################### ## initialize system ## ####################### #Get object list obj_list = self.get_objects() print(obj_list) obj_ans = raw_input('Are these the objects?(y/n)') while obj_ans != 'y': obj_list = self.get_objects() print(obj_list) obj_ans = raw_input('Are these the objects?(y/n)') objectList_msgs = String() objectList_msgs.data = '\n'.join(obj_list) self.objectList_pub.publish(objectList_msgs) #HACK # Start execution goToHome.goToARC(slowDown=self.goHomeSlow) # 1. Initialize robot state print("getPassiveVisionEstimate 'update hm sg', '', ", self.tote_ID) self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID) self.save_passive_vision_images(self.tote_ID) ################## ## Manipulation loop ## ################## if (rospy.get_param('have_robot') == True and self.is_record == True): directory = '/media/mcube/data/Dropbox (MIT)/rgrasp_dataset' assert (directory) self.gdr = GraspDataRecorder( directory=directory) #We instantiate the recorder else: self.gdr = None while True: print( '-----------------------------\n RUNNING GRASPING \n-----------------------------' ) self.weightSensor.calibrateWeights( withSensor=self.withSensorWeight) self.all_grasp_proposals = None self.run_grasping(container=self.tote_ID) # 4. Run grasping self.obj_ID = 'no_item' self.obj_weight = 0 print( '-----------------------------\n Execution_possible according to primitive = {} \n-----------------------------' .format(self.execution_possible)) if self.execution_possible != False: # 5. Check the weight self.weight_info = self.weightSensor.readWeightSensor( item_list=obj_list, withSensor=self.withSensorWeight, binNum=self.tote_ID, givenWeights=-11) self.obj_weight = self.weight_info['weights'] print('Detected weight:', self.obj_weight) if self.obj_weight > 10: # Weight need to be higher than 10g self.execution_possible = True #Identify object max_prob_index = (self.weight_info['probs']).tolist().index( max(self.weight_info['probs'])) self.obj_ID = obj_list[max_prob_index] if self.obj_ID == 'no_item' or self.execution_possible == None: self.execution_possible = False print( '-----------------------------\n Execution_possible according to weight = {} \n-----------------------------' .format(self.execution_possible)) #Manually change execution result self.execution_possible = raw_input('Did it succeeded?') #Publish experiment outcome grasp_status_msgs = sensor_msgs.msg.JointState() grasp_status_msgs.name = [ 'grasp_success', 'code_version', 'tote_ID', 'detected_weight' ] #grasp proposals, grasp_point, scores, score, grasp_status_msgs.position = [ self.execution_possible, self.version, self.tote_ID, self.obj_weight ] object_ID_msgs = String() object_ID_msgs.data = self.obj_ID if self.grasp_point is not None and rospy.get_param('have_robot'): self.grasp_status_pub.publish(grasp_status_msgs) self.objectType_pub.publish(object_ID_msgs) self.gdr.update_topic(key='grasp_status') self.gdr.update_topic(key='objectType') print( '-----------------------------\n Execution_possible = {} \n-----------------------------' .format(self.execution_possible)) if self.execution_possible: # 8. Placing self.fails_in_row = 0 self.bbox_info = fake_bbox_info_1( self.listener) #Give bounding box to the object self.bbox_info[7:10] = [ self.max_dimension, self.max_dimension, self.max_dimension ] self.planned_place() #TODO_M else: self.fails_in_row += 1 gripper.open() spatula.open() if self.grasp_point is not None: # 9. Add to bad grasp points self.bad_grasping_points.append(self.grasp_point) self.bad_grasping_times.append(time.time()) print( '************************\n End of grasping attempt \n***********************************' ) # 7. Update vision state of the tote self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID) self.save_passive_vision_images(self.tote_ID) if self.fails_in_row > 4: # 10. Failed too many times, take action print( 'The pick failed 4 times in a row, stopping if not infinite looping' ) if not self.infinite_looping: break # Finished stowing goToHome.goToARC(slowDown=self.goHomeSlow) print("Planner is done")
def goarc(): goToHome.goToARC(slowDown=True)
def unit_test_centers(): listener = tf.TransformListener() br = tf.TransformBroadcaster() #~ ~Build hand transform l1 = 0.0 l2 = 0.0 l3 = rospy.get_param("/gripper/spatula_tip_to_tcp_dist") tip_hand_transform = [l1, l2, l3, 0, 0, 0] gripperOriHome = [0, 1, 0, 0] q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] plans = [] withPause = True #~generate plans #loop through sections (bins) plans = [] goToHome.prepGripperPicking() goToHome.goToARC() for i in range(0, 9): binId = i collision_free_placing(binId, listener, isSuction=False) #~READ robot position rospy.sleep(.3) tcpPose = get_tcp_pose(listener, tcp_offset=l3) tcpPose[2] = rospy.get_param( '/bin' + str(i) + '_pose/z') #~- rospy.get_param('/bin'+str(i)+'/height') rospy.sleep(.3) q_initial = get_joints() rospy.sleep(1.0) ##move to edge of structure map_bin_id_to_ws_id = { 0: 0, 1: 1, 2: 2, 3: 3, 4: 0, 5: 0, 6: 4, 7: 4, 8: 5 } plan, qf, plan_possible = generatePlan(q_initial, tcpPose[0:3], gripperOriHome, tip_hand_transform, 'fast') if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' ##move to edge of structure map_bin_id_to_ws_id = { 0: 0, 1: 1, 2: 2, 3: 3, 4: 0, 5: 0, 6: 4, 7: 4, 8: 5 } plan, qf, plan_possible = generatePlan( q_initial, tcpPose[0:3] - np.array([0, 0, 0.2]), gripperOriHome, tip_hand_transform, 'fast', guard_on=WeightGuard(map_bin_id_to_ws_id[binId], threshold=100)) if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' executePlanForward(plans, False) if binId > 5: collision_free_placing(binId, listener, isSuction=False) plans = []
def calibrate_storage(): ################################################# #~ Generate Plans gripperOri = [] gripperOri.append([-1., 0., 0., 0.]) gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.]) gripperOri.append([0., -1., 0., 0.]) gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.]) #~build total list plan gripperOri_list = [] ori_list = [ 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0 ] for i in range(0, len(ori_list)): gripperOri_list.append(gripperOri[ori_list[i]]) #~positions pos = [] #~bin0 bin_pose = get_params_yaml('bin0_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, -0.46 + 0.19647, 0.41]) pos.append([0.75 + .12, bin_pose[1], 0.41]) pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41]) pos.append([1.19 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append( [1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin1 bin_pose = get_params_yaml('bin1_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41]) pos.append([0.75 + .12, bin_pose[1], 0.41]) pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41]) pos.append([1.19 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([ 0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05 ]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin2 bin_pose = get_params_yaml('bin2_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41]) pos.append([0.8 + .12, bin_pose[1], 0.41]) pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41]) pos.append([1.18 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append( [0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #################### ## Generate Plans ## #################### q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] tip_hand_transform = [0., 0., 0., 0., 0., 0.] dir_list = [ None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None ] point_counter = 0 #~generate plans plans = [] listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.5) goToHome.goToARC() #~ goToHome.prepGripperPicking() for i in range(0, len(pos)): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1
def calibrate_boxes(): ################################################# #~ Generate Plans gripperOri = [] gripperOri.append([-1., 0., 0., 0.]) gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.]) gripperOri.append([0., -1., 0., 0.]) gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.]) #~build total list plan gripperOri_list = [] ori_list = [ 0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0 ] for i in range(0, len(ori_list)): gripperOri_list.append(gripperOri[ori_list[i]]) #~positions pos = [] #~bin4 bin_pose = get_params_yaml('bin4_pose') pos.append([ bin_pose[0], bin_pose[1], 0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.1, -0.498443692923, 0.5512911677361 - .15]) pos.append([1.05, -0.599031984806, 0.5512911677361 - .15]) pos.append([1.14, -0.70, 0.5512911677361 - .15]) pos.append([1.2, -0.618239402771, 0.5512911677361 - .15]) pos.append([ bin_pose[0], bin_pose[1], 0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin5 bin_pose = get_params_yaml('bin5_pose') pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15]) pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15]) pos.append([0.79457449913, -0.70, 0.5512911677361 - .15]) pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin6 bin_pose = get_params_yaml('bin6_pose') pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0]) pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0]) pos.append([0.099, -0.68, 0.539945185184 + 0.0]) pos.append([0.15, -0.6, 0.539945185184 + 0.0]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.145160362124, -0.846666872501, 0.65]) pos.append([ bin_pose[0], bin_pose[1], 0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin7 bin_pose = get_params_yaml('bin7_pose') pos.append([ bin_pose[0], bin_pose[1], 0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0]) pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0]) pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0]) pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.203513264656, -0.86, 0.65]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin8 bin_pose = get_params_yaml('bin8_pose') pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.09, 0.68, 0.716840863228]) pos.append([-0.22, 0.62, 0.716840863228]) pos.append([-0.09, 0.5, 0.716840863228]) pos.append([0.1, 0.62, 0.716840863228]) pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.106256209314, 0.78, 0.8]) pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #################### ## Generate Plans ## #################### q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] tip_hand_transform = [0., 0., 0., 0., 0., 0.] dir_list = [ None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None ] point_counter = 0 #~generate plans plans = [] listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.5) goToHome.goToARC() goToHome.prepGripperPicking() ############################### ## Unwrap angles example use ## ############################### #~1) function to unwarp an angle to the range [-pi,pi] def unwrap(angle): if angle > np.pi: angle = np.mod(angle, 2 * np.pi) if angle > np.pi: angle = angle - 2 * np.pi elif angle < -np.pi: angle = np.mod(angle, -2 * np.pi) if angle < -np.pi: angle = angle + 2 * np.pi return angle for i in range(0, 16): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: #~2) unwrap angle of joint 6 only plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) #~3) set q_initial (for next plan) with the unwrapped angle joint state q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1 ################################## plans_col_free = [] plans_col_free, q_initial, plan_possible, motion = collision_free_plan( q_initial, plans_col_free, [0, 1, 0, 0], False, 6) executePlanForward(plans_col_free, True) for i in range(16, 32): #~ for i in range(0,16): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1 plans_col_free = [] plans_col_free, q_initial, plan_possible, motion = collision_free_plan( q_initial, plans_col_free, [0, 1, 0, 0], False, 8) executePlanForward(plans_col_free, True) for i in range(32, 40): #~ for i in range(16,24): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1
def run_experiments(self): ####################### ## initialize system ## ####################### #Get object list obj_list = self.get_objects() print(obj_list) #assert(False) obj_ans = raw_input('Are these the objects?(y/n)') while obj_ans != 'y': obj_list = self.get_objects() print(obj_list) obj_ans = raw_input('Are these the objects?(y/n)') objectList_msgs = String() objectList_msgs.data = '\n'.join(obj_list) #HACK self.objectList_pub.publish(objectList_msgs) goToHome.goToARC(slowDown=self.goHomeSlow) # 1. Initialize robot state # calibrate_gelsight() num_grasps = 0 if self.visionType == 'real': # 2. Passive vision update bins number_bins = 2 im_passive_vision = [] for bin_id in range(number_bins): print("getPassiveVisionEstimate 'update hm sg', '', ", bin_id) self.getPassiveVisionEstimate('update hm sg', '', bin_id) self.save_passive_vision_images(self.tote_ID) ################## ## stowing loop ## ################## if (rospy.get_param('have_robot') == True and self.is_record == True): directory = '/media/mcube/data/Dropbox (MIT)/rgrasp_dataset' assert (directory) self.gdr = GraspDataRecorder( directory=directory) #We instantiate the recorder else: self.gdr = None self.num_attempts = 0 self.num_attempts_failed = 0 while True: if num_grasps == 100: calibrate_gelsight() num_grasps = 0 self.num_attempts += 1 print( '-----------------------------\n RUNNING GRASPING \n-----------------------------' ) self.weightSensor.calibrateWeights( withSensor=self.withSensorWeight) self.all_grasp_proposals = None self.run_grasping(container=self.tote_ID) # 4. Run grasping self.obj_ID = 'no_item' self.obj_weight = 0 if self.execution_possible != False: # 5. Check the weight self.weight_info[ self.tote_ID] = self.weightSensor.readWeightSensor( item_list=obj_list, withSensor=self.withSensorWeight, binNum=self.tote_ID, givenWeights=-11) print( '-----------------------------\n Execution_possible according to primitive = {} \n-----------------------------' .format(self.execution_possible)) print('Detected weight:', self.weight_info[self.tote_ID]['weights']) self.obj_weight = self.weight_info[self.tote_ID]['weights'] if self.obj_weight > 10: self.execution_possible = True #Identify object max_prob_index = ( self.weight_info[self.tote_ID]['probs']).tolist().index( max(self.weight_info[self.tote_ID]['probs'])) self.obj_ID = obj_list[max_prob_index] if self.obj_ID == 'no_item': self.execution_possible = False if self.execution_possible == None: self.execution_possible = False # if self.visionType == 'real': # 7. Update vision state of the tote # self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID) # self.save_passive_vision_images(self.tote_ID) #Publish experiment outcome grasp_status_msgs = sensor_msgs.msg.JointState() grasp_status_msgs.name = [ 'grasp_success', 'code_version', 'tote_ID', 'detected_weight' ] #grasp proposals, grasp_point, scores, score, grasp_status_msgs.position = [ self.execution_possible, self.version, self.tote_ID, self.obj_weight ] object_ID_msgs = String() object_ID_msgs.data = self.obj_ID #, self.obj_weight] if self.grasp_point is not None and rospy.get_param('have_robot'): self.grasp_status_pub.publish(grasp_status_msgs) self.objectType_pub.publish(object_ID_msgs) self.gdr.update_topic(key='grasp_status') self.gdr.update_topic(key='objectType') print( '-----------------------------\n Execution_possible = {} \n-----------------------------' .format(self.execution_possible)) if self.execution_possible: # 8. Placing num_grasps += 1 self.fails_in_row = 0 self.bbox_info = fake_bbox_info_1( self.listener) #Give bounding box to the object self.bbox_info[7:10] = [ self.max_dimension, self.max_dimension, self.max_dimension ] self.planned_place() #TODO_M else: self.num_attempts_failed += 1 self.fails_in_row += 1 gripper.open() spatula.open() if self.grasp_point is not None: # 9. Add to bad grasp points self.bad_grasping_points.append(self.grasp_point) self.bad_grasping_times.append(time.time()) print( '***********************************************************') if self.visionType == 'real': # 7. Update vision state of the tote self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID) self.save_passive_vision_images(self.tote_ID) if self.fails_in_row > 3: # 10. Failed too many times, take action if self.infinite_looping and len(obj_list) > 1: #self.switch_tote() print( 'The pick failed 4 times in a row, switching totes, the tote id is {}' .format(self.tote_ID)) else: print('The pick failed 4 times in a row, stopping') break # Finished stowing goToHome.goToARC(slowDown=self.goHomeSlow) print("Planner is done")