def execute(self, userdata):
# wait for the specified duration or until we are asked to preempt
print("HOME")
res = m.move_to_named_pose('whole_arm', 'home')
if not res:
return 'failed'
rospy.sleep(2)
return 'succeeded'
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 choose(self, bin_id, userdata):
rospy.loginfo('MOVE TO STORAGE %s' % bin_id)
camera_locations = ['storage_%s', 'storage_%s_backup_1', 'storage_%s_backup_2']
poses = ['realsense_above_storage_system_%s', 'realsense_above_storage_system_%s_backup_1', 'realsense_above_storage_system_%s_backup_2']
userdata.data['chosen_bin'] = bin_id
userdata.data['camera_location'] = camera_locations[len(userdata.data['failed_views'][bin_id])] % bin_id
userdata.data['picking_from'] = 'storage_' + bin_id
userdata.data['visible_objects'] = self.visible_objects[bin_id]
res = m.move_to_named_pose('realsense', poses[len(userdata.data['failed_views'][bin_id])] % bin_id)
if not res:
return 'failed'
else:
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 execute(self, userdata):
rospy.loginfo('MOVE TO STOW TOTE')
userdata.data['camera_location'] = 'stow_tote'
userdata.data['picking_from'] = 'stow_tote'
userdata.data['visible_objects'] = userdata.data['item_locations'][
'tote']['contents']
if len(userdata.data['visible_objects']) == 0:
# We think we're done, but re-init all of the items and do a final check.
rospy.logerr('I think we\'re done.')
userdata.data['i_think_im_done'] = True
userdata.data['visible_objects'] = list(userdata.data['all_items'])
res = m.move_to_named_pose('realsense', 'realsense_above_stow_tote')
if not res:
return 'failed'
return 'succeeded'
def execute(self):
# Get the bottle pose from the camera.
rospy.loginfo('Moveing to view')
m.move_to_named_pose('whole_arm', 'view_checkerboard')
rospy.sleep(4)
poses = []
qt = tf.transformations.quaternion_from_euler(0.0, 0.0,
-3.141592653 / 2)
for i in range(16):
rospy.loginfo('GET OBJECT POSE')
b_req = cartesian_calibration.srv.GetACoorRequest()
b_req.data_set = "measured"
b_req.object_idx = i
b_res = self.pose_service.call(b_req)
p = b_res.object_pose.pose
# p.orientation.x = qt[0]
# p.orientation.y = qt[1]
# p.orientation.z = qt[2]
# p.orientation.w = qt[3]
object_global = t.convert_pose(p, self.camera_reference_frame,
'global_xyz_link')
# object_global.position.z = min(0.80,p.position.z)
poses.append(object_global)
m.move_to_named_pose('realsense', 'tool_change_position')
m.move_to_named_pose('wrist_only', 'sucker')
for p in poses:
if p.position.x == 0 and p.position.y == 0 and p.position.z == 0:
rospy.logerr('Failed to find an object. %s' % i)
# return 'failed'
continue
print("MOVE SUCKER TO ABOVE OBJECT")
p = t.align_pose_orientation_to_frame(p, 'global_xyz_link',
'sucker_endpoint')
t.publish_pose_as_transform(p, 'global_xyz_link', 'MOVE HERE', 0.5)
res = m.move_to_global(p.position.x - 0.02,
p.position.y + 0.01,
p.position.z - 0.02,
'sucker',
orientation=p.orientation)
rospy.sleep(4)
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):
if userdata.data['task'] == 'stow':
if userdata.data['empty_space_capture'].get('storage_A') is None:
# We need to update storage_A
res = m.move_to_named_pose('realsense',
'realsense_above_storage_system_A')
if not res:
return 'failed'
pc, pose = self.get_point_cloud_and_position('storage_A')
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture']['storage_A'] = {
'point_cloud': pc,
'pose': pose
}
if userdata.data['empty_space_capture'].get('storage_B') is None:
# We need to update storage_A
res = m.move_to_named_pose('realsense',
'realsense_above_storage_system_B')
if not res:
return 'failed'
pc, pose = self.get_point_cloud_and_position('storage_B')
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture']['storage_B'] = {
'point_cloud': pc,
'pose': pose
}
elif userdata.data['task'] == 'pick':
for b in boxes:
if userdata.data['empty_space_capture'].get(b) is None:
bd = boxes[b]
# Find centre of box in x direction
x_pos = (bd['right'] + bd['left']) / 2
x_pos = min(x_pos, REALSENSE_MAX_GLOBAL_X)
x_pos = max(x_pos, REALSENSE_MIN_GLOBAL_X)
y_pos = 0.819 # This stops us hitting the gripper on the frame.
res = m.move_to_global(x_pos, y_pos, 0.575, 'realsense')
if not res:
return 'failed'
rospy.set_param('/storage_specs/%s/h_high' % b, 180)
rospy.set_param('/storage_specs/%s/h_inside' % b, True)
rospy.set_param('/storage_specs/%s/h_low' % b, 0)
rospy.set_param('/storage_specs/%s/s_high' % b, 255)
rospy.set_param('/storage_specs/%s/s_low' % b, 0)
rospy.set_param('/storage_specs/%s/v_high' % b, 255)
rospy.set_param('/storage_specs/%s/v_low' % b, 0)
rospy.set_param('/storage_specs/%s/x_max' % b,
(x_pos - bd['right']) + 0.02)
rospy.set_param('/storage_specs/%s/x_min' % b,
(x_pos - bd['left']) - 0.02)
rospy.set_param('/storage_specs/%s/y_max' % b,
(y_pos - bd['bottom']) + 0.02)
rospy.set_param('/storage_specs/%s/y_min' % b,
(y_pos - bd['top']) - 0.04)
rospy.set_param('/storage_specs/%s/z_max' % b, 1000)
rospy.set_param('/storage_specs/%s/z_min' % b, 0.4)
pc, pose = self.get_point_cloud_and_position(b)
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture'][b] = {
'point_cloud': pc,
'pose': pose
}
self.pc_pub.publish(pc)
return 'succeeded'
def execute(self, userdata):
first_attamept = True
label = userdata.data['item_to_pick']['label']
suck_level = item_meta[label].get('suction_pressure', 3)
set_suck_level(suck_level)
USE_ANGLED = False
position_limits = POSITION_LIMITS[
userdata.data['picking_from']]['sucker']
# Get the tote weight before the pick
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] != 'grip_suck':
# 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 'suck_failed'
for object_global in userdata.data['item_to_pick']['grasp_poses']:
USE_ANGLED = decide_angled(object_global, position_limits)
object_global.position.x += 0.015
object_global.position.y -= 0.03
if USE_ANGLED:
rospy.logerr('ANGLED PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
object_global.position.z -= 0.02 # The endpoint is half way through the suction cup, so move up slightly
#t.publish_pose_as_transform(object_global, 'global_xyz_link', 'ORIGINAL', seconds=2)
translation = [
object_global.position.x, object_global.position.y,
object_global.position.z
]
qm = object_global.orientation
q = (qm.x, qm.y, qm.z, qm.w)
R = tft.quaternion_matrix(q)
P = tft.projection_matrix([0, 0, 0], [0, 0, 1]) # XY plane
pose_vec = R[:, 2] # Z portion of matrix.
proj_vec = np.dot(
P,
pose_vec)[:3] # Projected onto XY plane as a unit vector.
proj_vec_unit = tft.unit_vector(proj_vec)
y_unit = [0, 1, 0] # Unit vector in the y-direction.
yaw_angle = np.arccos(np.dot(
proj_vec_unit, y_unit)) # Angle to the grasp around Z.
pitch_angle = np.arcsin(
tft.vector_norm(proj_vec)) # Angle to tilt the suction cup
if pitch_angle > 1.2:
pitch_angle = 1.2
# Only get absolute angle from above. Check the direction of the vector.
if R[0, 2] > 0:
yaw_angle = -1 * yaw_angle
# Create a quaternion with the right angles (note rotations are applied in the rotated frame in zyx order)
q = tft.quaternion_from_euler(yaw_angle, 0, -1 * pitch_angle,
'rzyx')
if yaw_angle > 1.571 or yaw_angle < -1.571:
# Rotate by 180 to normalise to the sucker workspace.
q2 = tft.quaternion_from_euler(0, 0, 3.1415)
q = tft.quaternion_multiply(q, q2)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(e[0], e[1], 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]
R = tft.quaternion_matrix(q)
t.publish_tf_quaterion_as_transform(translation,
q,
'global_xyz_link',
'MOVE_HERE',
seconds=0.5)
# Create a pre-grasp pose away from the object.
T = tft.translation_matrix(translation)
T2 = tft.translation_matrix((0, 0, -0.05))
F = tft.concatenate_matrices(T, R, T2)
pre_grasp_t = tft.translation_from_matrix(F)
pre_grasp_q = tft.quaternion_from_matrix(F)
#t.publish_tf_quaterion_as_transform(pre_grasp_t, pre_grasp_q, 'global_xyz_link', 'PRE', seconds=5.0)
grasp_orientation = Quaternion()
grasp_orientation.x = q[0]
grasp_orientation.y = q[1]
grasp_orientation.z = q[2]
grasp_orientation.w = q[3]
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
if first_attamept:
# Move the whole wrist and then select the sucker.
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 SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
else:
# Straighten up the sucker from the last attempt and move into position
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
min(object_global.position.x, SUCKER_MAX_GLOBAL_X),
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT) - 0.02,
SUCKER_MIN_GLOBAL_Y + 0.02),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
rospy.loginfo("MOVE TO PRE-GRASP")
res = m.move_to_global(pre_grasp_t[0],
pre_grasp_t[1],
min(pre_grasp_t[2],
SUCKER_MAX_GLOBAL_Z),
'sucker',
orientation=grasp_orientation,
velo_scale=0.4)
if not res:
return 'failed'
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
object_global.position.y,
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=grasp_orientation,
velo_scale=0.3)
if not res:
return 'failed'
else:
# Straight Down Suck
rospy.logerr('STRAIGHT PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
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',
seconds=0.5)
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
# We only need to select the sucker on the first attempt.
# Can cause it to break becasue the controllers don't switch properly
# if there's no actual movment and we change controllers too quickly. Not sure why, something to do with timing.
if first_attamept:
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT),
SUCKER_MIN_Y_STRAIGHT) - SUCKER_REALSENSE_Y_OFFSET,
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
# Weird async movement bug.
rospy.sleep(1.0)
else:
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT), SUCKER_MIN_Y_STRAIGHT),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
# Correct the rotation if required.
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.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(0, 0, e[2])
pca_yaw.x = q[0]
pca_yaw.y = q[1]
pca_yaw.z = q[2]
pca_yaw.w = q[3]
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
v_scale = 0.25
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_point_type'] == 'rgb_centroid':
v_scale = 0.1
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT),
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=pca_yaw,
velo_scale=v_scale)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'sucker_endpoint', 'sucker')
rospy.sleep(0.5)
elif not suction.check_pressure_sensor():
# If it's not already on, give it a sec.
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor(
) #query pressure sensor for state, has the object been sucked correctly
rospy.loginfo("PRESSURE SENSOR STATUS: %s" % suction_state)
if not suction_state and not bail:
set_suck_level(3)
# Just move down a touch and double check
move_amount = 0.02
if USE_ANGLED:
move_amount = 0.035
if object_global.position.z < (SUCKER_MAX_GLOBAL_Z):
move_amount = min(
move_amount,
SUCKER_MAX_GLOBAL_Z - object_global.position.z)
res = m.move_relative(0,
0,
move_amount,
'sucker_endpoint',
'sucker',
plan_time=0.5)
else:
res = True
if res:
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor()
if suction_state:
#userdata.data['last_failed'] = None
return 'succeeded'
else:
suction.pump_off()
# Run out of grasp poses.
suction.pump_off()
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'suck_grip':
return 'try_gripper'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'suck_failed'
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')