def planned_place(self, fixed_container=None):
fixed_container = [
1 - self.tote_ID
] #TODO_M: planner only accepts bins 1,2,3 and names them as 0,1,2
if self.PlacingPlanner.visionType == 'real': #Update HM
self.PlacingPlanner.update_real_height_map(fixed_container[0])
drop_pose = self.PlacingPlanner.place_object_local_best(
None, containers=fixed_container
) #TODO_M : change placing to return drop_pose
print('drop_pose', drop_pose)
#~frank hack: drop pose
drop_pose = get_params_yaml('bin' + str(fixed_container[0]) + '_pose')
# Place object using grasping
self.rel_pose, self.BoxBody = vision_transform_precise_placing_with_visualization(
self.bbox_info, viz_pub=self.viz_array_pub, listener=self.listener)
grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=fixed_container[0],
flag=2,
withPause=self.withPause,
rel_pose=self.rel_pose,
BoxBody=self.BoxBody,
place_pose=drop_pose,
viz_pub=self.viz_array_pub,
is_drop=False,
recorder=self.gdr)
def callFakeGrasping(self, prob, container):
print('------- DOING GRASPING ------- ')
print(' grasp_point = ', self.grasp_point)
grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
flag=0,
withPause=False,
viz_pub=self.viz_array_pub)
f = random.choice(os.listdir(
self.FAKE_GRASPING_DIR)) #Get fake output for the primitive
with open(os.path.join(self.FAKE_GRASPING_DIR, f), 'r') as infile:
self.grasping_output = json.load(infile)
self.grasping_output = self.grasping_output['primitive_output']
#Random decides if execution possible
self.grasping_output['execution_possible'] = (np.random.rand() <= prob)
self.execution_possible = self.grasping_output['execution_possible']
def callFakeGrasping(self, prob, container):
print('------- DOING GRASPING ------- ')
print(' grasp_point = ', self.grasp_point)
grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=False,
viz_pub=self.viz_array_pub)
if self.is_control:
#find new and improved grasp points
back_img_list = self.controller.capture_images()
best_grasp_dict = self.controller.control_policy(
back_img_list, smirror=self.smirror)
# self.controller.visualize_actions(with_CAM = False)
# self.controller.visualize_best_action()
print best_grasp_dict['delta_pos']
#go for new grasp Point
self.grasping_output = grasp_correction(
self.grasp_point, best_grasp_dict['delta_pos'], self.listener,
self.br)
retrieve(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=False,
viz_pub=self.viz_array_pub,
ws_object=self.weightSensor)
f = random.choice(os.listdir(
self.FAKE_GRASPING_DIR)) #Get fake output for the primitive
with open(os.path.join(self.FAKE_GRASPING_DIR, f), 'r') as infile:
self.grasping_output = json.load(infile)
self.grasping_output = self.grasping_output['primitive_output']
#Random decides if execution possible
self.grasping_output['execution_possible'] = (np.random.rand() <= prob)
self.execution_possible = self.grasping_output['execution_possible']
def getBestGraspingPoint(self, container):
self.GetGraspPoints(num_points=100, container=container)
self.execution_possible = False
if self.num_pick_proposals == 0: #If no grasp point available
self.grasp_point = None
self.grasp_score = 0
return
#Find the best point that it is not in collision:
num_attempts = 0
num_it = 0
while not self.execution_possible and num_attempts < 20 and num_it < self.num_pick_proposals: #Each time try at most 20 attempts
grasp_point = copy.deepcopy(
list(self.all_pick_proposals[num_it][:]))
num_it += 1
#If we already know it is a bad point, we do not try it again
if grasp_point in self.bad_grasping_points:
return
#Check if in collision
num_attempts += 1
checked_output = grasp(objInput=grasp_point,
listener=self.listener,
br=self.br,
isExecute=False,
binId=container,
flag=0,
withPause=False,
viz_pub=self.viz_array_pub)
if checked_output['execution_possible']:
self.grasp_score = copy.deepcopy(self.all_pick_scores[num_it -
1])
self.grasp_point = copy.deepcopy(grasp_point)
print('Best grasp point:', grasp_point, ' with score: ',
self.grasp_score, 'in bin: ', container)
return
if grasp_point is not None:
self.bad_grasping_points.append(grasp_point)
self.bad_grasping_times.append(-1)
print('NONE OF THE GRASPING POINTS WORKS')
self.grasp_point = None
self.grasp_score = 0
print('Best grasp point:', grasp_point, ' with score: ',
self.grasp_score, 'in bin: ', container)
return
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_msgs)
comments_msgs = String()
comments_msgs.data = self.experiment_description
self.experiment_comments_pub.publish(comments_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):
p.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)
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
flag=0,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
self.execution_possible = self.grasping_output['execution_possible']
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
#~Publish grasp proposal information
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_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)
else:
print('There are no grasp proposal')
self.execution_possible = False
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
self.back_img_list = self.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.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)
num_it = 0
print('gripper open: ', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
while 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, isShake=False)
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, open_hand=False)
'''
# Motion heuristic
initial_dz = 0.05
dz = .022 #should be related to object length
ik.helper.move_cart(dz=initial_dz)
rospy.sleep(0.5)
ik.helper.move_cart(dz=-initial_dz)
rospy.sleep(0.5)
gripper.move(90)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
#should be done in the direction of the gripper plane
dx = 0.038 #.02*num_it
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
print('gripper_open', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
if is_in_wrong_pose:
gripper.move(90)
rospy.sleep(0.5)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
self.gdr.save_data_recorded = False
num_it += 1
is_in_wrong_pose = (
gripper.getGripperopening() < 0.04
) #and (gripper.getGripperopening() > 0.015)
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,
isShake=False)
self.execution_possible = self.retrieve_output['execution_possible']
def getBestGraspingPoint(self, container):
self.GetGraspPoints(num_points=100, container=container)
self.execution_possible = False
if self.num_pick_proposals == 0: #If no grasp point available
self.grasp_point = None
self.grasp_score = 0
return
#Find the best point that it is not in collision:
num_attempts = 0
num_it = 0
while not self.execution_possible and num_attempts < 20 and num_it < self.num_pick_proposals: #Each time try at most 20 attempts
grasp_point = copy.deepcopy(
list(self.all_pick_proposals[num_it][:]))
num_it += 1
#If we already know it is a bad point, we do not try it again
if grasp_point in self.bad_grasping_points:
return
#Check if in collision
num_attempts += 1
grasp_output = grasp(objInput=grasp_point,
listener=self.listener,
br=self.br,
isExecute=False,
binId=container,
withPause=False,
viz_pub=self.viz_array_pub)
retrieve_output = retrieve(listener=self.listener,
br=self.br,
isExecute=False,
binId=container,
withPause=False,
viz_pub=self.viz_array_pub,
ws_object=self.weightSensor)
if grasp_output['execution_possible'] and retrieve_output[
'execution_possible']:
self.grasp_score = copy.deepcopy(self.all_pick_scores[num_it -
1])
self.grasp_point = copy.deepcopy(grasp_point)
#Visualize grasp
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,
np.asarray([self.grasp_point]), self.listener, self.br,
True)
print('Best grasp point:', grasp_point, ' with score: ',
self.grasp_score, 'in bin: ', container)
return
if grasp_point is not None:
self.bad_grasping_points.append(grasp_point)
self.bad_grasping_times.append(-1)
print('NONE OF THE GRASPING POINTS WORKS')
self.grasp_point = None
self.grasp_score = 0
return
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.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)
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)
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 = self.controller.control_policy(
back_img_list, smirror=self.smirror)
# self.controller.visualize_actions(with_CAM = False)
# self.controller.visualize_best_action(with_CAM = False)
#save network information action_dict and best_action_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)
#WE UNPAUSE THE RECORDER
self.gdr.replay_recording()
#go for new grasp Point
self.grasping_output = grasp_correction(
self.grasp_point, best_grasp_dict['delta_pos'],
self.listener, self.br)
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 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']