def abort():
print(
'[Recorder] ***************************************************'
)
print('[Recorder] Sensor Issue: The sensor {} is empty. Abort!'.
format(term))
print(
'[Recorder] ***************************************************'
)
print('info_dict', info_dict)
gripper.open()
spatula.open()
sys.exit()
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 reset_gripper():
gripper.open()
spatula.open()
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")
