def execute(self):
# Get the bottle pose from the camera.
square = 0.1175
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
for i in range(6):
for j in range(7):
m.move_to_global(curr.position.x + square * i,
curr.position.y + square * j, curr.position.z,
'gripper')
rospy.sleep(4)
def execute(self, userdata):
suction.pump_off()
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
rospy.loginfo('MOVE UP TO SAFE POSITION')
res = m.move_to_global(curr.position.x, curr.position.y, 0.505,
'realsense')
if not res:
rospy.logerr('Failed to move to safe position.')
return 'failed'
rospy.sleep(10.0)
return 'succeeded'
def execute(self, userdata):
rospy.loginfo('MOVE TO SAFE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
res = m.move_to_global(curr.position.x,
min(curr.position.y, TOOL_CHANGE_SAFE_Y),
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('TESTING THAT THE WRIST MOVES')
res = m.move_to_named_pose('wrist_only', 'test_joints')
if not res:
rospy.logerr('Wrist Failed to Move')
return 'failed'
return 'succeeded'
def get_point_cloud_and_position(self, storage):
try:
pc = rospy.wait_for_message(
'/realsense_wrist/depth_registered/points', PointCloud2, 5.0)
pc = rospy.wait_for_message(
'/realsense_wrist/depth_registered/points', PointCloud2, 5.0)
except rospy.ROSException:
vision.reset_vision()
return None, None
rospy.loginfo('\tRemoving storage system from point cloud')
fil_request = CropCloudRequest()
fil_request.input_cloud = pc
fil_request.crop_image.data = False
fil_request.crop_cloud.data = True
fil_request.storage_name.data = storage
res_fil = self.filter_service.call(fil_request)
pc = res_fil.segmented_cloud_posonly
pose = t.current_robot_pose('global_xyz_link',
'realsense_wrist_rgb_optical_frame')
return pc, pose
def execute(self, userdata):
gripper.open_gripper
rospy.loginfo('TOOL CHANGE')
rospy.loginfo('\tMove up to safe height')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
res = m.move_to_global(curr.position.x, curr.position.y, 0.505,
'realsense')
if not res:
rospy.logerr('Failed to move to safe position.')
return 'failed'
rospy.loginfo('\tMove to tool change position')
res = m.move_to_named_pose('realsense', 'tool_change_position')
if not res:
rospy.logerr('Failed to move to tool_change_position')
return 'failed'
if self.tool == 'gripper':
rospy.loginfo('\tSelect Gripper')
tool_change_named_pose = 'tool_change_gripper'
elif self.tool == 'sucker':
rospy.loginfo('\tSelect Sucker')
tool_change_named_pose = 'tool_change_sucker'
elif self.tool == 'neutral':
# Allow us to select neutral
rospy.loginfo('\tSelect Neutral')
tool_change_named_pose = 'tool_change_neutral'
else:
rospy.logerr('Incorrect tool name for tool change')
return 'failed'
res = m.move_to_named_pose('whole_arm', tool_change_named_pose)
if not res:
rospy.logerr('Failed to move to %s' % tool_change_named_pose)
return 'failed'
return 'succeeded'
def execute(self, userdata):
suction.set_suck_level(0)
suction.pump_off()
gripper.open_gripper()
rospy.sleep(0.5)
userdata.data['move_objects_mode'] = False
userdata.data['move_objects_between_bins_mode'] = None
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
# Don't hit the top bar.
if curr.position.y >= TOOL_CHANGE_SAFE_Y:
curr.position.y = TOOL_CHANGE_SAFE_Y
# Don't hit the down bars.
if curr.position.y >= TOOL_CHANGE_DANGEROUS_Y:
# We might hit the front frame so move away from it.
if curr.position.x >= TOOL_CHANGE_X_MAX:
curr.position.x = TOOL_CHANGE_X_MAX
if curr.position.x <= TOOL_CHANGE_X_MIN:
curr.position.x = TOOL_CHANGE_X_MIN
res = m.move_to_global(curr.position.x,
min(curr.position.y, TOOL_CHANGE_SAFE_Y),
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('CHANGE TO NEUTRAL WRIST NEUTRAL')
res = m.move_to_named_pose('wrist_only', 'neutral')
if not res:
return 'failed'
return 'succeeded'
def execute(self, userdata):
# wait for the specified duration or until we are asked to preempt
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
rospy.loginfo('MOVE UP TO DROP POSITION')
into_bin = 'B' # Default
weight_before = -1
drop_orientation = Quaternion() # Default straight
drop_orientation.x = 0
drop_orientation.y = 0
drop_orientation.z = 0
drop_orientation.w = 1
x_pos = None
y_pos = None
z_pos = None
if userdata.data['task'] == 'stow':
#bins = ['storage_B', 'storage_A']
bins = ['storage_B'] # Only B for finals.
elif userdata.data['task'] == 'pick':
if userdata.data['move_objects_between_bins_mode'] is not None:
bins = [
'storage_' +
userdata.data['move_objects_between_bins_mode']
]
else:
l = userdata.data['item_to_pick']['label']
box_id = None
for o in userdata.data['order']:
if l in o['contents']:
box_id = o['size_id']
bins = [box_id]
if box_id is None:
rospy.logerr("NO BOX FOR ITEM")
return 'failed'
else:
return 'failed'
for bin_id in bins:
# Try and find free space based on the point cloud.
# Try both bins.
if bin_id not in userdata.data['empty_space_capture']:
continue
pc = userdata.data['empty_space_capture'][bin_id].get(
'point_cloud', None)
if pc is None:
# Make sure we actually have one.
continue
req = ObjectPlacementPoseFromCloudRequest()
req.cropped_cloud = userdata.data['empty_space_capture'][bin_id][
'point_cloud']
req.camera_to_world = userdata.data['empty_space_capture'][bin_id][
'pose']
l = userdata.data['item_to_pick']['label']
dimensions = item_meta[l]['dimensions']
dimensions.sort()
req.object_longest_side_length.data = dimensions[2]
req.object_middlest_side_length.data = dimensions[1]
req.object_shortest_side_length.data = dimensions[0]
req.resolution.data = 0.01
res = self.free_space_service.call(req)
if not res.success.data:
continue
x_pos = res.x.data
y_pos = res.y.data
z_pos = res.z.data - 0.13
if res.degrees_to_global_y.data > 89:
res.degrees_to_global_y.data = 89
if res.degrees_to_global_y.data < -89:
res.degrees_to_global_y.data = -89
tq = tft.quaternion_from_euler(
0, 0, res.degrees_to_global_y.data * 3.14 / 180.0)
drop_orientation.x = tq[0]
drop_orientation.y = tq[1]
drop_orientation.z = tq[2]
drop_orientation.w = tq[3]
t.publish_tf_quaterion_as_transform(
(x_pos, y_pos, z_pos), tq, 'global_xyz_link', 'DROP_HERE', 0.5)
userdata.data['empty_space_capture'][bin_id] = None
break
if userdata.data['task'] == 'stow':
into_bin = bin_id[-1]
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
x_pos = 0.350
y_pos = 0.440
z_pos = 0.900
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
if item_meta[userdata.data['item_to_pick']['label']].get(
'ignore_weight') == True:
rospy.logerr("Overriding drop point for floppy item")
x_pos = 0.350
y_pos = 0.210
z_pos = 0.800
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
position_limits = POSITION_LIMITS['storage_' + into_bin]['gripper']
x_pos = max(x_pos, position_limits['x_min'])
x_pos = min(x_pos, position_limits['x_max'])
y_pos = max(y_pos, position_limits['y_min'])
y_pos = min(y_pos, position_limits['y_max'])
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
weight_before = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
weight_before = -1
elif userdata.data['task'] == 'pick' and userdata.data[
'move_objects_between_bins_mode'] is None:
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right']) / 2
y_pos = (box['top'] + box['bottom']) / 2
z_pos = 0.95
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
if item_meta[userdata.data['item_to_pick']['label']].get(
'ignore_weight') == True:
rospy.logerr('Overriding drop point for floppy item')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right']) / 2
y_pos = box['top'] - 0.02
z_pos = 0.9
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
x_pos = max(x_pos, SUCKER_MIN_GLOBAL_X)
x_pos = min(x_pos, SUCKER_MAX_GLOBAL_X)
y_pos = min(y_pos, SUCKER_MAX_Y_STRAIGHT)
elif userdata.data['task'] == 'pick' and userdata.data[
'move_objects_between_bins_mode'] is not None:
drop_pos = POSITION_LIMITS[
'storage_' +
userdata.data['move_objects_between_bins_mode']]['centre']
x_pos = drop_pos[0]
y_pos = drop_pos[1]
z_pos = 0.9
into_bin = userdata.data['move_objects_between_bins_mode']
res = m.move_to_global(x_pos,
y_pos,
GRIPPER_LIFT_HEIGHT_STRAIGHT + 0.01,
'gripper',
orientation=drop_orientation)
if not res:
rospy.logerr('Failed to move to tote')
return 'failed'
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
pose = gmsg.Pose()
pose.position.x = curr.position.x
pose.position.y = curr.position.y
pose.position.z = z_pos
pose.orientation = drop_orientation
if userdata.data['task'] == 'stow':
# We can use the scales to check our movement.
scales_topic = 'storage_%s_scales/weight' % into_bin
(res, stopped, bail) = m.move_to_global_monitor_weight(
pose.position.x,
pose.position.y,
pose.position.z,
'gripper',
scales_topic,
orientation=pose.orientation,
velo_scale=0.4)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'gripper_endpoint', 'gripper')
else:
# No scales, perform a normal movement
res = m.move_to_global(pose.position.x,
pose.position.y,
pose.position.z,
'gripper',
orientation=pose.orientation,
velo_scale=0.4)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
res = gripper.open_gripper()
if not res:
rospy.logerr('Failed to open gripper')
return 'failed'
rospy.sleep(1.0)
userdata.data['last_drop_bin'] = into_bin
if weight_before != -1 and userdata.data['task'] == 'stow':
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
if (weight.data - weight_before) < 5:
rospy.logerr('No weight change, we must have dropped it.')
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
# Undo an added weight reclassification if it happened.
if userdata.data['weight_reclassifications']:
if userdata.data['weight_reclassifications'][-1][
1] == userdata.data['item_to_pick']['label']:
userdata.data[
'weight_reclassifications'] = userdata.data[
'weight_reclassifications'][:-1]
# Don't update the item locations.
return 'succeeded'
rospy.logerr("SUCCESSFUL DROP")
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
if userdata.data[
'task'] == 'pick' and userdata.data['weight_before'] != -1:
# Make sure the weight in the source storage system is still down.
scales_topic = userdata.data['picking_from'] + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr(
'Failed to get weight from %s, ignoring weight check')
else:
dw = userdata.data['weight_before'] - weight.data
if dw < 4:
# The item must have fallen out before we got to the box.
# Don't update the item locations
rospy.logerr(
'The source storage system didn\'t change weight, we probably dropped the item'
)
return 'succeeded'
# Update item locations.
if userdata.data['task'] == 'stow':
cur_label = userdata.data['item_to_pick']['label']
try:
userdata.data['item_locations']['tote']['contents'].remove(
cur_label)
except ValueError:
# The item wasn't meant to be in the tote, maybe we thought we already moved it.
# Amend the list.
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['bins']:
if b['bin_id'] == into_bin:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data[
'move_objects_between_bins_mode'] is None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['boxes']:
if b['size_id'] == box_id:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data[
'move_objects_between_bins_mode'] is not None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
if b['bin_id'] == userdata.data[
'move_objects_between_bins_mode']:
b['contents'].append(cur_label)
userdata.data['move_objects_between_bins_mode'] = None
json_functions.save_file(userdata.data['task'],
'item_location_file_out.json',
userdata.data['item_locations'])
return 'succeeded'
def execute(self, userdata):
# Only do it once.
move_objects_mode = userdata.data['move_objects_mode']
userdata.data['move_objects_mode'] = False
rospy.loginfo('LIFTING OBJECT WITH SUCKER')
curr = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
# Update current pose with new orientation.
curr.position.x = min(curr.position.x, SUCKER_MAX_GLOBAL_X - 0.01)
curr.position.y = min(curr.position.y, SUCKER_MAX_Y_STRAIGHT)
curr.position.y = max(curr.position.y, SUCKER_MIN_GLOBAL_Y + 0.01)
suck_height = curr.position.z
curr.position.z = SUCKER_LIFT_HEIGHT
#t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
co = curr.orientation
q = [co.x, co.y, co.z, co.w]
e = tft.euler_from_quaternion(q)
q = tft.quaternion_from_euler(0, 0, e[2])
qm = gmsg.Quaternion()
qm.x = 0
qm.y = 0
qm.z = 0
qm.w = 1
v_scale = 0.3
if item_meta[userdata.data['item_to_pick']['label']]['weight'] > 0.5:
# Heavy Item
v_scale = 0.15
res = m.move_to_global(curr.position.x, curr.position.y, curr.position.z, 'sucker', orientation=qm, velo_scale=v_scale)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
if not suction.check_pressure_sensor():
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
return 'suck_failed'
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if move_objects_mode and userdata.data['move_objects_between_bins_mode'] is None:
mid_x, mid_y = POSITION_LIMITS[userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.15
else:
curr.position.x += 0.15
if curr.position.y > mid_y:
curr.position.y -= 0.2
else:
curr.position.y += 0.2
res, _, _ = m.move_to_global_monitor_weight(curr.position.x, curr.position.y, suck_height - 0.13, 'sucker', scales_topic, velo_scale=0.2)
return 'suck_failed'
if userdata.data['weight_before'] < 0:
rospy.logerr('No pre-pick weight, ignoring weight check')
return 'succeeded'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
return 'succeeded'
userdata.data['weight_after'] = weight.data
dw = userdata.data['weight_before'] - weight.data
if item_meta[userdata.data['item_to_pick']['label']].get('weight')*1000.0 < 5:
# We can't do a weight check.
return 'succeeded'
if userdata.data['weight_failures'].get(userdata.data['item_to_pick']['label'], 0) > 3 and (len(userdata.data['visible_objects']) < 6 or userdata.data.get('i_think_im_done')):
# We've failed weight check on this item heaps of times and we're almost finished so just pick it.
rospy.logerr('I am ignoring the weight check since it has failed too many times.')
return 'succeeded'
if dw < 4:
# We haven't got anything, go back to the camera position and try again.
rospy.logerr('No detected change in weight, failed grasp.')
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
return 'suck_failed'
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
return 'succeeded'
weight_correct, possible_matches = w.weight_matches(userdata.data['item_to_pick']['label'], dw, userdata.data['visible_objects'], 3)
if not weight_correct:
rospy.logerr(possible_matches)
if userdata.data.get('i_think_im_done', False) == True:
# Don't do weight reclassifications at the end.
pass
elif userdata.data['move_objects_between_bins_mode'] is not None:
pass
elif len(possible_matches) == 1 and possible_matches[0][0] < 5:
# There's only one thing it could actually be!
if userdata.data['task'] == 'stow' or (userdata.data['task'] == 'pick' and possible_matches[0][1] in userdata.data['wanted_items']):
rospy.logerr("I'M CHANGING THE OBJECT TO %s" % possible_matches[0][1])
userdata.data['weight_reclassifications'].append( (userdata.data['item_to_pick']['label'], possible_matches[0][1]) )
userdata.data['item_to_pick']['label'] = possible_matches[0][1]
suck_level = item_meta[possible_matches[0][1]].get('suction_pressure', 3)
suction.set_suck_level(suck_level)
return 'succeeded'
elif len(possible_matches) > 1 and userdata.data['task'] == 'stow':
userdata.data['double_check_objects'] = [i[1] for i in possible_matches]
suck_pos = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
suck_pos.position.z = suck_height
userdata.data['original_pick_position'] = suck_pos
return 'secondary_check'
# Put the item back down and bail.
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
if userdata.data['item_to_pick']['label'] not in userdata.data['weight_failures']:
userdata.data['weight_failures'][userdata.data['item_to_pick']['label']] = 1
else:
userdata.data['weight_failures'][userdata.data['item_to_pick']['label']] += 1
rospy.logerr("WEIGHT DOESN'T MATCH: MEASURED: %s, EXPECTED: %s" % (dw, item_meta[userdata.data['item_to_pick']['label']].get('weight')*1000.0))
# Move towards the centre.
mid_x, mid_y = POSITION_LIMITS[userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.08
else:
curr.position.x += 0.08
if curr.position.y > mid_y:
curr.position.y -= 0.08
else:
curr.position.y += 0.08
res, _, _ = m.move_to_global_monitor_weight(curr.position.x, curr.position.y, suck_height - 0.13, 'sucker', scales_topic, velo_scale=0.1)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'suck_failed'
return 'succeeded'
def execute(self, userdata):
gripper.open_gripper()
position_limits = POSITION_LIMITS[userdata.data['picking_from']]['gripper']
# Get the weight before the pick
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if item_meta[userdata.data['item_to_pick']['label']]['grasp_type'] != 'suck_grip':
# If we've just tried sucking, then don't re-weigh
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
userdata.data['weight_before'] = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
userdata.data['weight_before'] = -1
if item_meta[userdata.data['item_to_pick']['label']]['grasp_point_type'] == 'rgb_centroid':
rospy.logerr("Aborting RGB Centroid pick because there aren't any scales.")
return 'grip_failed'
# No way to detect failure yet, so just use one grasp point.
object_global = userdata.data['item_to_pick']['grasp_poses'][0]
object_global.position.x += 0.015
object_global.position.y -= 0.03
object_global.position.x = max(object_global.position.x, position_limits['x_min'])
object_global.position.x = min(object_global.position.x, position_limits['x_max'])
object_global.position.y = max(object_global.position.y, position_limits['y_min'])
object_global.position.y = min(object_global.position.y, position_limits['y_max'])
object_global.position.z = min(object_global.position.z, position_limits['z_max'])
t.publish_pose_as_transform(object_global, 'global_xyz_link', 'MOVE HERE', 0.5)
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
# res = m.move_to_global(curr.position.x, curr.position.y, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
res = m.move_to_global(min(object_global.position.x, REALSENSE_MAX_GLOBAL_X), max(min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT) - 0.02, SUCKER_MIN_GLOBAL_Y + 0.02) - SUCKER_REALSENSE_Y_OFFSET, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING GRIPPER')
res = m.move_to_named_pose('wrist_only', 'gripper')
if not res:
return 'failed'
object_global.position.z += 0.015
pca_yaw = userdata.data['item_to_pick']['pca_yaw']
q = (pca_yaw.x, pca_yaw.y, pca_yaw.z, pca_yaw.w)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.57:
e[2] -= 3.14
if e[2] < -1.57:
e[2] += 3.14
if e[2] > 1.55: # Stop gimbal lock and weird joint configurations.
e[2] = 1.55
if e[2] < -1.55:
e[2] = -1.55
q = tft.quaternion_from_euler(0, 0, e[2])
object_global.orientation.x = q[0]
object_global.orientation.y = q[1]
object_global.orientation.z = q[2]
object_global.orientation.w = q[3]
rospy.loginfo('MOVE GRIPPER TO ABOVE OBJECT')
res = m.move_to_global(object_global.position.x, object_global.position.y, max(object_global.position.z - 0.2, GRIPPER_MIN_GLOBAL_Z), 'gripper', orientation=object_global.orientation)
if not res:
return 'failed'
rospy.loginfo('MOVING TO OBJECT')
v_scale = 0.1
if item_meta[userdata.data['item_to_pick']['label']]['grasp_point_type'] == 'rgb_centroid':
# Don't trust the depth.
v_scale = 0.1
if userdata.data['item_to_pick']['label'] == 'mesh_cup':
v_scale = 0.05
(res, stopped, bail) = m.move_to_global_monitor_weight(object_global.position.x, object_global.position.y,
min(object_global.position.z, GRIPPER_MAX_GLOBAL_Z), 'gripper', scales_topic, velo_scale=v_scale, orientation=object_global.orientation)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'gripper_endpoint', 'gripper')
# rospy.sleep(0.5)
rospy.loginfo('CLOSE THE GRIPPER')
gripper.close_gripper()
rospy.sleep(0.5)
return('succeeded')
SIMULATION TOOL CHANGE ANGLE TEST SCRIPT
'''
if __name__ == '__main__':
move_error = 'Failed to complete a move'
move_success = 'Move State: '
tool = 'gripper'
if tool is 'gripper':
#TOOL CHANGE TO 45 DEGREES
rospy.init_node('my_ros_node')
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
pose = gmsg.Pose()
qt = tf.transformations.quaternion_from_euler(0.785398, 0.0, 1.57)
pose.orientation.x = qt[0]
pose.orientation.y = qt[1]
pose.orientation.z = qt[2]
pose.orientation.w = qt[3]
rospy.loginfo('MOVE UP TO 45 DEGREES POSITION')
res = m.move_to_global(curr.position.x,
curr.position.y,
0.6,
'gripper',
orientation=pose.orientation)
if not res:
rospy.logerr(move_error)
rospy.loginfo(move_success + str(res))
def execute(self, userdata):
# Only do it once.
move_objects_mode = userdata.data['move_objects_mode']
userdata.data['move_objects_mode'] = False
# Lift the gripper up slowly so that it gets a good grip.
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
res = m.move_to_global(curr.position.x,
curr.position.y,
max(curr.position.z - 0.05,
GRIPPER_MIN_GLOBAL_Z),
'gripper',
velo_scale=0.02)
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
# Update current pose with new orientation.
grip_height = curr.position.z
curr.position.z = GRIPPER_LIFT_HEIGHT_STRAIGHT
#t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
res = m.move_to_global(curr.position.x,
curr.position.y,
curr.position.z,
'gripper',
orientation=curr.orientation,
velo_scale=0.5)
if not res:
rospy.logerr('Failed to perform a move (lift object gripper)')
return 'failed'
hold_gripper()
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if move_objects_mode and userdata.data[
'move_objects_between_bins_mode'] is None:
mid_x, mid_y = POSITION_LIMITS[
userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.15
else:
curr.position.x += 0.15
if curr.position.y > mid_y:
curr.position.y -= 0.15
else:
curr.position.y += 0.15
res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
curr.position.y,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
return 'grip_failed'
if userdata.data['weight_before'] == -1:
rospy.logerr('No pre-pick weight, ignoring weight check')
return 'succeeded'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
return 'succeeded'
userdata.data['weight_after'] = weight.data
dw = userdata.data['weight_before'] - weight.data
if item_meta[userdata.data['item_to_pick']['label']].get(
'weight') * 1000.0 < 5:
# We can't do a weight check.
return 'succeeded'
if userdata.data['weight_failures'].get(
userdata.data['item_to_pick']['label'],
0) > 3 and (len(userdata.data['visible_objects']) < 6
or userdata.data.get('i_think_im_done')):
# We've failed weight check on this item heaps of times and we're almost finished so just pick it.
return 'succeeded'
if dw < 4:
# We haven't got anything, go back to the camera position and try again.
rospy.logerr('No detected change in weight, failed grasp.')
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
return 'try_sucker'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'grip_failed'
if item_meta[userdata.data['item_to_pick']['label']].get(
'ignore_weight') == True:
return 'succeeded'
weight_correct, possible_matches = w.weight_matches(
userdata.data['item_to_pick']['label'], dw,
userdata.data['visible_objects'], 3)
if not weight_correct:
rospy.logerr(possible_matches)
if userdata.data.get('i_think_im_done', False) == True:
# Don't do weight reclassifications at the end.
pass
elif userdata.data['move_objects_between_bins_mode'] is not None:
pass
elif len(possible_matches) == 1 and possible_matches[0][0] < 5:
# There's only one thing it could actually be!
if userdata.data['task'] == 'stow' or (
userdata.data['task'] == 'pick'
and possible_matches[0][1]
in userdata.data['wanted_items']):
rospy.logerr("I'M CHANGING THE OBJECT TO %s" %
possible_matches[0][1])
userdata.data['weight_reclassifications'].append(
(userdata.data['item_to_pick']['label'],
possible_matches[0][1]))
userdata.data['item_to_pick']['label'] = possible_matches[
0][1]
return 'succeeded'
# Put the item back down and bail.
if userdata.data['item_to_pick']['label'] not in userdata.data[
'weight_failures']:
userdata.data['weight_failures'][userdata.data['item_to_pick']
['label']] = 1
else:
userdata.data['weight_failures'][userdata.data['item_to_pick']
['label']] += 1
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
rospy.logerr("WEIGHT DOESN'T MATCH: MEASURED: %s, EXPECTED: %s" %
(dw, item_meta[userdata.data['item_to_pick']
['label']].get('weight') * 1000.0))
# Move towards the centre.
mid_x, mid_y = POSITION_LIMITS[
userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.08
else:
curr.position.x += 0.08
if curr.position.y > mid_y:
curr.position.y -= 0.08
else:
curr.position.y += 0.08
res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
curr.position.y,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'grip_failed'
return 'succeeded'
def execute(self, userdata):
into_bin = 'B'
weight_before = -1
drop_orientation = Quaternion()
drop_orientation.x = 0
drop_orientation.y = 0
drop_orientation.z = 0
drop_orientation.w = 1
drop_velo_scale = 0.5
x_pos = None
y_pos = None
z_pos = None
if userdata.data['task'] == 'stow':
#bins = ['storage_B', 'storage_A']
bins = ['storage_B'] # Only B for finals.
elif userdata.data['task'] == 'pick':
if userdata.data['move_objects_between_bins_mode'] is not None:
bins = ['storage_' + userdata.data['move_objects_between_bins_mode']]
else:
l = userdata.data['item_to_pick']['label']
box_id = None
for o in userdata.data['order']:
if l in o['contents']:
box_id = o['size_id']
bins = [box_id]
if box_id is None:
rospy.logerr("NO BOX FOR ITEM")
return 'failed'
else:
return 'failed'
for bin_id in bins:
if bin_id not in userdata.data['empty_space_capture']:
continue
pc = userdata.data['empty_space_capture'][bin_id].get('point_cloud', None)
if pc is None:
# Make sure we actually have one.
continue
req = ObjectPlacementPoseFromCloudRequest()
req.cropped_cloud = userdata.data['empty_space_capture'][bin_id]['point_cloud']
req.camera_to_world = userdata.data['empty_space_capture'][bin_id]['pose']
l = userdata.data['item_to_pick']['label']
dimensions = item_meta[l]['dimensions']
dimensions.sort()
req.object_longest_side_length.data = dimensions[2]
req.object_middlest_side_length.data = dimensions[1]
req.object_shortest_side_length.data = dimensions[0]
req.resolution.data = 0.01
if max(item_meta[l]['dimensions']) > 0.12:
drop_velo_scale = 0.15
res = self.free_space_service.call(req)
if not res.success.data:
continue
x_pos = res.x.data
y_pos = res.y.data
z_pos = res.z.data - 0.13
if res.degrees_to_global_y.data == 90 and (x_pos < 0.75 or y_pos < 0.75):
res.degrees_to_global_y.data = -90 # Rotate away from the vacuum hose, unless we're in the to left coner.
if res.degrees_to_global_y.data > 89: # Stops the robot doing weird gimbal lock things.
res.degrees_to_global_y.data = 89
if res.degrees_to_global_y.data < -89:
res.degrees_to_global_y.data = -89
tq = tft.quaternion_from_euler(0, 0, res.degrees_to_global_y.data * 3.14/180.0)
drop_orientation.x = tq[0]
drop_orientation.y = tq[1]
drop_orientation.z = tq[2]
drop_orientation.w = tq[3]
t.publish_tf_quaterion_as_transform((x_pos, y_pos, z_pos), tq, 'global_xyz_link', 'DROP_HERE', 0.5)
userdata.data['empty_space_capture'][bin_id] = None
break
if userdata.data['task'] == 'stow':
into_bin = bin_id[-1]
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
x_pos = 0.350
y_pos = 0.440
z_pos = 0.900
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
rospy.logerr("Overriding drop point for floppy item")
x_pos = 0.350
y_pos = 0.210
z_pos = 0.800
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
position_limits = POSITION_LIMITS[bin_id]['sucker']
x_pos = max(x_pos, position_limits['x_min'])
x_pos = min(x_pos, position_limits['x_max'])
y_pos = max(y_pos, position_limits['y_min'])
y_pos = min(y_pos, position_limits['y_max'])
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
weight_before = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
weight_before = -1
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is None:
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right'])/2
y_pos = (box['top'] + box['bottom'])/2
z_pos = 0.95
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
rospy.logerr('Overriding drop point for floppy item')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right'])/2
y_pos = box['top'] - 0.02
z_pos = 0.9
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
x_pos = max(x_pos, SUCKER_MIN_GLOBAL_X)
x_pos = min(x_pos, SUCKER_MAX_GLOBAL_X)
y_pos = min(y_pos, SUCKER_MAX_Y_STRAIGHT)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is not None:
drop_pos = POSITION_LIMITS['storage_' + userdata.data['move_objects_between_bins_mode']]['centre']
x_pos = drop_pos[0]
y_pos = drop_pos[1]
if userdata.data['move_objects_between_bins_mode'] == 'A':
x_pos += 0.08
elif userdata.data['move_objects_between_bins_mode'] == 'B':
x_pos -= 0.08
z_pos = 0.9
into_bin = userdata.data['move_objects_between_bins_mode']
move_velo_scale = 1.0
if item_meta[userdata.data['item_to_pick']['label']]['weight'] > 0.5:
# Heavy Item
move_velo_scale = 0.5
curr = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
res = m.move_to_global(x_pos, y_pos, curr.position.z + 0.01, 'sucker', orientation=drop_orientation, velo_scale=move_velo_scale)
if not res:
rospy.logerr('Failed to move to drop point')
return 'failed'
# if suction.check_pressure_sensor() == False:
# rospy.logerr('No suction pressure, we probably dropped it')
# # Undo an added weight reclassification if it happened.
# if userdata.data['weight_reclassifications']:
# if userdata.data['weight_reclassifications'][-1][1] == userdata.data['item_to_pick']['label']:
# userdata.data['weight_reclassifications'] = userdata.data['weight_reclassifications'][:-1]
# return 'succeeded'
if userdata.data['task'] == 'stow':
# We can use the scales to check our movement
scales_topic = 'storage_%s_scales/weight' % into_bin
res, stopped, bail = m.move_to_global_monitor_weight(x_pos, y_pos, z_pos, 'sucker', scales_topic, velo_scale=drop_velo_scale)
if not res:
rospy.logerr('Failed to perform a move')
#return 'failed' # Don't fail, just continue because the item will probably end up in the storage system.
if bail:
m.move_relative(0, 0, -0.04, 'sucker_endpoint', 'sucker')
else:
# No scales for pick task, just normal move.
res = m.move_to_global(x_pos, y_pos, z_pos, 'sucker', velo_scale=0.2)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
set_suck_level(0)
suction.pump_off()
rospy.sleep(1.0)
userdata.data['last_drop_bin'] = into_bin
if weight_before != -1 and userdata.data['task'] == 'stow':
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
if (weight.data - weight_before) < 5:
rospy.logerr('No weight change, we must have dropped it.')
# Don't update the item locations.
if userdata.data['weight_reclassifications']:
if userdata.data['weight_reclassifications'][-1][1] == userdata.data['item_to_pick']['label']:
userdata.data['weight_reclassifications'] = userdata.data['weight_reclassifications'][:-1]
return 'succeeded'
rospy.logerr("SUCCESSFUL DROP")
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
if userdata.data['task'] == 'pick' and userdata.data['weight_before'] != -1:
# Make sure the weight in the source storage system is still down.
scales_topic = userdata.data['picking_from'] + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
else:
dw = userdata.data['weight_before'] - weight.data
if dw < 4:
# The item must have fallen out before we got to the box.
# Don't update the item locations
rospy.logerr('The source storage system didn\'t change weight, we probably dropped the item')
return 'succeeded'
# Update item locations.
if userdata.data['task'] == 'stow':
cur_label = userdata.data['item_to_pick']['label']
try:
userdata.data['item_locations']['tote']['contents'].remove(cur_label)
except ValueError:
# The item wasn't meant to be in the tote, maybe we thought we already moved it.
# Amend the list.
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['bins']:
if b['bin_id'] == into_bin:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['boxes']:
if b['size_id'] == box_id:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is not None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
if b['bin_id'] == userdata.data['move_objects_between_bins_mode']:
b['contents'].append(cur_label)
userdata.data['move_objects_between_bins_mode'] = None
# Update the output file.
json_functions.save_file(userdata.data['task'], 'item_location_file_out.json', userdata.data['item_locations'])
return 'succeeded'