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 callback(data):
s = str(data)
print(s)
data = re.split('data: "| "| |"', s)
print(len(data))
print(data)
if len(data) == 3:
if data[1] == 'o':
gripper.open()
elif data[1] == 'c':
gripper.close()
else:
gripper.stop()
elif len(data) == 8:
for i in range(1, 7):
query[i] = float(data[i])
dequeue()
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 go_up_arc():
gripper.open()
ik.helper.move_cart(dz=.3)
goToHome.goToARC()
def go_up():
gripper.open()
ik.helper.move_cart(dz=.3)
def reset_gripper():
gripper.open()
spatula.open()
def openGripper():
# call gripper node, open
gripper.open()
return 1
#!/usr/bin/env python
import gripper
from ik.helper import Timer
gripper.homing()
gripper.open(speed=50)
with Timer('close50'):
gripper.close(speed=50)
with Timer('open50'):
gripper.open(speed=50)
with Timer('close100'):
gripper.close(speed=100)
with Timer('open100'):
gripper.open(speed=100)
with Timer('close200'):
gripper.close(speed=200)
with Timer('open200'):
gripper.open(speed=200)
with Timer('close400'):
gripper.close(speed=400)
with Timer('open400'):
gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
#!/usr/bin/env python
import gripper
from ik.helper import Timer
gripper.homing()
gripper.open(speed=50)
with Timer('close50'):
gripper.close(speed=50)
with Timer('open50'):
gripper.open(speed=50)
with Timer('close100'):
gripper.close(speed=100)
with Timer('open100'):
gripper.open(speed=100)
with Timer('close200'):
gripper.close(speed=200)
with Timer('open200'):
gripper.open(speed=200)
with Timer('close400'):
gripper.close(speed=400)
with Timer('open400'):
gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
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")
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
grasp_noise_msgs = Float32MultiArray()
grasp_noise_msgs.data = self.grasp_noise
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_msgs)
self.grasp_noise_pub.publish(grasp_noise_msgs)
comments_msgs = String()
comments_msgs.data = self.experiment_description
experiment_type_msgs = String()
experiment_type_msgs.data = self.experiment_type
self.experiment_comments_pub.publish(comments_msgs)
self.experiment_type_pub.publish(experiment_type_msgs)
if self.grasp_point is None:
print(
'It was suppose to do grasping, but there is no grasp proposal'
)
self.execution_possible = False
return
if self.visionType != 'real':
self.callFakeGrasping(prob=0.8, container=container)
return
#~visualize grasp proposals
markers_msg = MarkerArray()
m0 = createDeleteAllMarker('pick_proposals')
markers_msg.markers.append(m0)
for i in range(10):
self.proposal_viz_array_pub.publish(markers_msg)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, self.all_grasp_proposals,
self.listener, self.br)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
self.listener, self.br, True)
#execute for grasp. Stop when the gripper is closed
if self.is_control:
back_img_list = self.controller.capture_images()
#self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]
self.background_images = self.capture_images()
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
if self.is_record == True:
self.gdr.save_item(item_name='grasp_noise_std_dev',
data=self.grasp_std)
self.gdr.save_item(item_name='grasp_noise', data=self.grasp_noise)
is_in_wrong_pose = (gripper.getGripperopening() < 0.03) #raw_input()
if is_in_wrong_pose:
self.retrieve_output = retrieve(
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor)
gripper.open()
gripper.close()
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
if self.is_control:
if gripper.getGripperopening() > 0.017:
print('[Planner]: ', gripper.getGripperopening())
# WE PAUSE THE RECOORDER TO SAVE DATA
#self.gdr.pause_recording()
#find new and improved grasp points
best_grasp_dict, initial_score = self.controller.control_policy(
back_img_list,
smirror=self.smirror,
use_COM=self.use_COM,
use_raw=self.use_raw)
# self.controller.visualize_actions(with_CAM = False)
#self.controller.visualize_best_action(with_CAM = False)
#pdb.set_trace()
#save network information action_dict and best_action_dict
#WE UNPAUSE THE RECORDER
#self.gdr.replay_recording()
self.gdr.save_item(item_name='initial_score',
data=initial_score)
self.gdr.save_item(item_name='best_grasp_dict',
data=best_grasp_dict)
self.gdr.save_item(item_name='action_dict',
data=self.controller.action_dict)
self.gdr.save_item(item_name='best_action_dict',
data=self.controller.best_action_dict)
#go for new grasp PointgraspPose
self.grasping_output = grasp_correction(
self.grasp_point, best_grasp_dict['delta_pos'],
self.listener, self.br)
second_best_grasp_dict, final_score = self.controller.control_policy(
back_img_list,
smirror=self.smirror,
use_COM=self.use_COM,
use_raw=self.use_raw)
self.gdr.save_item(item_name='final_score', data=final_score)
self.gdr.save_item(item_name='second_best_grasp_dict',
data=second_best_grasp_dict)
self.gdr.save_data_recorded = True
else:
self.gdr.save_data_recorded = False
#frank hack for double grasping
if self.experiment_type == 'is_data_collection':
if gripper.getGripperopening() > 0.017:
# self.grasping_output = grasp_correction(self.grasp_point, np.array([0,0,0]), self.listener, self.br)
self.gdr.save_data_recorded = True
else:
self.gdr.save_data_recorded = False
self.retrieve_output = retrieve(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor)
self.execution_possible = self.retrieve_output['execution_possible']
def openGripper():
# call gripper node, open
gripper.open()
return 1