def release_safe(listener, isExecute=True, withPause=False):
plans = []
#Initialize parameters
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
bin_pose = ik.helper.get_params_yaml('bin' + str(0) + '_pose')
UpdistFromBinFast = 0.15 - 0.1
#Initial configuration of robot
q_initial = ik.helper.get_joints()
targetPose = ik.helper.get_tcp_pose(listener,
tcp_offset=spatula_tip_to_tcp_dist)
#Desired new safe position above binNum
targetPose[2] = bin_pose[2] + UpdistFromBinFast
#################
## Build plans ##
#################
#~1. Open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
plans.append(grasp_plan)
#~2. Open spatula
grasp_plan = EvalPlan('spatula.open()')
plans.append(grasp_plan)
#~3. Move up
plan, qf, plan_possible = generatePlan(q_initial,
targetPose[0:3],
targetPose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='go_safe_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
print('[Release Safe] Plans failed:')
#execute plan_name
if isExecute and plan_possible:
executePlanForward(plans, withPause)
def push_back(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId='crayola_64_ct',
bin_contents=[
'paper_mate_12_count_mirado_black_warrior', 'crayola_64_ct',
'expo_dry_erase_board_eraser'
],
robotConfig=None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute=True,
withPause=True):
#~ *****************************************************************
obj_dim = get_obj_dim(objId)
obj_dim = np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width = 0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
(shelf_position,
shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
shelf_pose_tfm = np.array(shelf_pose_tfm_list)
shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
#~ print '[PushBack] shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos = shelf_pose_tfm[0:3, 3]
#~ print '[PushBack] shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm = np.array(obj_pose_tfm_list)
obj_pose_orient = obj_pose_tfm[0:3, 0:3]
obj_pose_pos = obj_pose_tfm[0:3, 3]
#~ print '[PushBack] obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
#~ print '[PushBack] obj_X'
#~ print obj_X
obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
#~ print '[PushBack] obj_Y'
#~ print obj_Y
obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
#~ print '[PushBack] obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
obj_pose_orient_norm = obj_pose_orient_norm.transpose()
#~ print '[PushBack] obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
#~ print '[PushBack] proj'
#~ print proj_vecY
max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir] > 0:
hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
else:
hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]
#~ print '[PushBack] hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
la.norm(hand_norm_vec))
angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180 / np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
# print '[PushBack] dim along hand norm'
# print obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)
max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir] > 0:
fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
else:
fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]
#~ print '[PushBack] fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
# print '[PushBack] obj_dim_along_fingAxis'
# print obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)
max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
np.fabs(proj_vecZ))
Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]
#~ print '[PushBack] Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis = obj_dim[Zaxis_dir]
hand_Y = np.cross(hand_norm_vec, fing_axis_vec)
diag_dist = la.norm(
np.array([obj_dim_along_hand_norm, obj_dim_along_fingAxis]))
# print'req obj diag dist=', diag_dist
#~ *****************************************************************
# Find the maximum distance to which we will push
other_diag = np.zeros(len(bin_contents))
for num_bin_contents in range(0, len(bin_contents)):
if bin_contents[num_bin_contents] != objId:
temp_obj_dim = get_obj_dim(objId)
other_diag[num_bin_contents] = la.norm(
np.array([temp_obj_dim[0], temp_obj_dim[1]]))
max_diag, max_obj_ID = np.max(np.fabs(other_diag)), np.argmax(
np.fabs(other_diag))
# print '[PushBack] max_obj_ID',max_diag
# print '[PushBack] max_obj_ID', bin_contents[max_obj_ID]
#*******************************************************************
bin_inner_cnstr = get_bin_inner_cnstr()
#*******************************************************************
#Check if we can do left push
diag_factor = 0.75
obj_left_edge = obj_pose_pos[1] + diag_factor * diag_dist / 2.0
binLeftWall = bin_inner_cnstr[binNum][1]
binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
'map', listener)
binRightWall = bin_inner_cnstr[binNum][0]
binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
'shelf', 'map', listener)
left_gap = binLeftWall_world.pose.position.y - obj_left_edge
# print '[PushBack] left_gap=', left_gap
fing_clearance = 0.010 # It's like 5 mm on both sides of finger
bin_width = binLeftWall_world.pose.position.y - binRightWall_world.pose.position.y
tolerance_in_push = 0.020
if left_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False, False)
if left_gap > fing_width + fing_clearance:
left_push = True
print '[PushBack] left push is possible'
else:
left_push = False
print '[PushBack] left push is NOT possible'
# check if we can do right push
obj_right_edge = obj_pose_pos[1] - diag_factor * diag_dist / 2.0
right_gap = obj_right_edge - binRightWall_world.pose.position.y
# print '[PushBack] right_gap', right_gap
fing_clearance = 0.010 # It's like 5 mm on both sides of finger
if right_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False, False)
if right_gap > fing_width + fing_clearance:
right_push = True
print '[PushBack] right push is possible'
else:
right_push = False
print '[PushBack] right push is NOT possible'
#*******************************************************************
#Find Push Points for left push
bin_face_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map',
listener)
tcp_Z_off = 0.005 #lower down from center of the bin
push_tcp_Z_shelfFrame = (
(bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
2.0) - tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform(
[0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z
push_side_off = 1.0
if left_push:
left_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
left_pushPt_Y = binLeftWall_world.pose.position.y - push_side_off * left_gap
# left_Y_lim=binLeftWall_world.pose.position.y-
left_pushPt_Z = deepcopy(push_tcp_Z)
left_push_Pt = np.array([left_pushPt_X, left_pushPt_Y, left_pushPt_Z])
print '[PushBack] expected left push clearance=', binLeftWall_world.pose.position.y - left_pushPt_Y - fing_width / 2.0
#Find Push Points for right push
if right_push:
right_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
right_pushPt_Y = binRightWall_world.pose.position.y + push_side_off * right_gap
right_pushPt_Z = deepcopy(push_tcp_Z)
right_push_Pt = np.array(
[right_pushPt_X, right_pushPt_Y, right_pushPt_Z])
print '[PushBack] expected right push clearance=', right_pushPt_Y - binRightWall_world.pose.position.y - fing_width / 2.0
#Execute push trajectories
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient = spatula_down_orient
push_hand_opening = 0.100
joint_topic = '/joint_states'
#~ *************************************************************
#Execure left push trajectory
if left_push:
# plan store
left_plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (left push)'
left_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (left push)'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0
push_l2 = 0.0
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
distFromShelf = -0.015
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, left_push_Pt[1], InbinPt.pose.position.z
] #matching world_Y of first push pt
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
left_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening * 1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
left_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X = bin_face_pos_world.pose.position.x + 0.43
push_stop_worldX = back_bin_X - max_diag - 0.010
left_pushStop_Pt_pos = deepcopy(left_push_Pt)
left_pushStop_Pt_pos[0] = push_stop_worldX
#Push until the end
planner = IK(q0=q_initial,
target_tip_pos=left_pushStop_Pt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
left_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualize()
pauseFunc(withPause)
left_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
left_plans[len(left_plans) - numOfPlan - 1].executeBackward()
#*******************************************************************
if right_push:
# plan store
right_plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (right push)'
right_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (right push)'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0
push_l2 = 0.0
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
distFromShelf = -0.015
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, right_push_Pt[1], InbinPt.pose.position.z
] #matching world_Y of first push pt
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
right_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening * 1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
right_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X = bin_face_pos_world.pose.position.x + 0.42
push_stop_worldX = back_bin_X - max_diag - 0.010
right_pushStop_Pt_pos = deepcopy(right_push_Pt)
right_pushStop_Pt_pos[0] = push_stop_worldX
#Push until the end
planner = IK(q0=q_initial,
target_tip_pos=right_pushStop_Pt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
right_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualize()
pauseFunc(withPause)
right_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
right_plans[len(right_plans) - numOfPlan - 1].executeBackward()
if right_push or left_push:
push_possible = True
else:
push_possible = False
return (push_possible, False)
def grasp(objInput,
listener,
br,
isExecute=True,
objId='expo_eraser',
binId=0,
flag=0,
withPause=False,
rel_pose=None,
BoxBody=None,
place_pose=None,
viz_pub=None,
is_drop=True,
update_command=None,
recorder=None):
#########################################################
# fhogan and nikhilcd
# Description:
#~2017 picking primitive
#
#~Usage
#~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
#
#~Parameters:
#
#~Inputs:
# 1) objInput: Input from vision or RViz (list of 12, or 7)
# 2) listener: Input from vision or RViz (list of 12, or 7)
# 3) br: Input from vision or RViz (list of 12, or 7)
# 4) isExecute: execute the robot motion if possible or not? (Bool)
# 5) objId: name of object (str)
# 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
# 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
# 8) withPause: Pause between robot motions (Bool)
# 9) rel_pose: object pose relative to link_6
# 10) BoxBody: Bounding box points resolved in link 6 frame
# 11) place_pose: desired position of bounding box
# 12) viz_pub: desired position of bounding box
#
#~Output:
# Dictionary of following keys:
# 1) grasp_possible: Does the object fit in the hand? (True/False)
# 2) plan_possible: Has the plan succeeded? (True/False)
# 3) execution_possible: Is there anything in the grasp? (True/False)
# 4) gripper_opening: gripper opening after grasp attempt (in meters)
# 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
# 6) gelsight_data: [] , dummy for now
#########################################################
#rospy.logdebug(msg, *args)
#rospy.loginfo(msg, *args)
#rospy.logwarn(msg, *args)
#rospy.logerr(msg, *args)
#rospy.logfatal(msg, *args)
###############################
## Pring input variables for ##
###############################
rospy.logdebug('[Picking] Starting primitive with flag: %d' % flag)
rospy.logdebug('[Picking] objInput %s', objInput)
rospy.logdebug('[Picking] objId %s', objId)
rospy.logdebug('[Picking] flag %s', flag)
rospy.logdebug('[Picking] withPause %s', withPause)
rospy.logdebug('[Picking] rel_pose %s', rel_pose)
rospy.logdebug('[Picking] BoxBody %s', BoxBody)
rospy.logdebug('[Picking] place_pose %s', place_pose)
########################
## Initialize values ##
########################
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
plans = []
plans_grasping = []
plans_grasping2 = []
plans_guarded = []
plans_placing = []
graspPose = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
weightSensor = ws_prob.WeightSensor()
liftoff_pub = rospy.Publisher('/liftoff_time',
std_msgs.msg.String,
queue_size=10,
latch=False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPose': graspPose,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
#########################
## Constant parameters ##
#########################
UpdistFromBinFast = 0.15 # Margin above object during "move to a location"
UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location"
gripperOriHome = [0, 1, 0, 0]
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
vision_pos[2] = 0.17786528
###############################
## Picking primitive flag: 0 ##
###############################
if flag == 0:
# ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]), listener, br, False)
#~Define reference frames
world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
listener=listener, br=br)
#~get grasp pose and gripper opening from vision
if len(objInput) == 12:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
objInput)
elif len(objInput) == 7:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
objInput, objId, listener, br)
#~build gripper orientation matrix 3x3
hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
hand_orient_norm = hand_orient_norm.transpose()
#~Grasp relocation script
hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
theta = euler[2] - np.pi
colFreePose = collisionFree(graspPos,
binId=binId,
listener=listener,
br=br,
finger_opening=grasp_width,
safety_margin=0.00,
theta=theta)
graspPos[0:2] = colFreePose[0:2]
#################################
## GENERATE PLANS OF PRIMITIVE ##
#################################.
#~ Start from home position
# if isExecute:
# q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
# scorpion.back()
#~0. Move to a location above the bin, Rotate gripper to grasp orientation
pregrasp_targetPosition = graspPos
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_safe_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1. Move to to surface of bin
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_top_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~2. Open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
plans.append(grasp_plan)
#~ 3. Open spatula
grasp_plan = EvalPlan('spatula.open()')
plans.append(grasp_plan)
grasp_targetPosition = deepcopy(graspPos)
# rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)
#~4. sleep
sleep_plan = EvalPlan('rospy.sleep(0.2)')
plans.append(sleep_plan)
#~5. perform guarded move down
grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
'/tote/height') + 0.000 #~frank hack for safety
if binId == 0:
grasp_targetPosition[2] = grasp_targetPosition[2] + 0.005
plan, qf, plan_possible = generatePlan(q_initial,
grasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'faster',
guard_on=WeightGuard(
binId, threshold=100),
plan_name='guarded_pick')
if plan_possible:
plans_guarded.append(plan)
q_initial = qf
else:
return compose_output()
#~7. Close spatula
grasp_plan = EvalPlan('spatula.close()')
plans_guarded.append(grasp_plan)
#~8. Close gripper
grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 50))
plans_guarded.append(grasp_plan)
#~9. sleep
sleep_plan = EvalPlan('rospy.sleep(0.2)')
plans_guarded.append(sleep_plan)
#~10. Move to a location above the bin
plan, qf, plan_possible = generatePlan(q_initial,
vision_pos,
delta_vision_pose[3:7],
tip_hand_transform,
'fastest',
plan_name='retrieve_object')
if plan_possible:
plans_grasping.append(plan)
q_initial = qf
else:
return compose_output()
################
## EXECUTION ###
################
if isExecute and plan_possible:
executePlanForward(plans, withPause)
###We start recording now
lasers.start(binId)
recorder.start_recording(
action='grasping',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
#Execute guarded move
rospy.set_param('is_record', True)
executePlanForward(plans_guarded, withPause)
rospy.set_param('is_record', False)
#Publish liftoff time
liftoff_msgs = std_msgs.msg.String()
liftoff_msgs.data = 'Liftoff (Robot command)'
liftoff_pub.publish(liftoff_msgs)
#Execute non-guarded grasp plan move
executePlanForward(plans_grasping, withPause)
###We stop recording
recorder.stop_recording(save_action=True)
lasers.stop(binId)
#~Check if picking success
low_threshold = 0.0035
high_threshold = 0.015
if isExecute and plan_possible:
rospy.sleep(2)
gripper_opening = gripper.getGripperopening()
if gripper_opening > high_threshold:
rospy.loginfo('[Picking] ***************')
rospy.loginfo(
'[Picking] Pick Successful (Gripper Opening Test)')
rospy.loginfo('[Picking] ***************')
execution_possible = True
else:
rospy.loginfo('[Picking] ***************')
rospy.loginfo(
'[Picking] Pick Inconclusive (Gripper Opening Test)')
rospy.loginfo('[Picking] ***************')
execution_possible = None
elif not isExecute:
execution_possible = plan_possible
return compose_output()
#############
## Placing ##
#############
elif flag == 2:
#if primitive called without placing arguments, go to center of bin
if rel_pose == None:
drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
drop_pose[3:7] = gripperOriHome
else:
drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
place_pose, viz_pub,
listener, br)
#~ Adjust height according to weigth
if is_drop:
drop_offset = 0.15
else:
drop_offset = 0.002
#~set drop pose target z position to be bottom of bin
drop_pose[2] = bin_pose[2] - rospy.get_param(
'tote/height') + drop_offset
#~Predrop: go to top middle bin surface fast without guarded move
predrop_pos = np.array(drop_pose[0:3])
predrop_pos[2] = bin_pose[2] + 0.05
######################
## Generate plans ##
######################
#~move safe to placing bin number
rospy.logdebug(
'[Picking] Collision free placing to binId %s and position %s',
binId, predrop_pos)
# if isExecute:
# q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
# if update_command is not None:
# update_command.execute()
#~0.go up to avoid collision with tote walls
current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
current_pose[2] = current_pose[2] + 0.15
plan, qf, plan_possible = generatePlan(q_initial,
current_pose[0:3],
current_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_up')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~0.1.fast motion to bin surface high
predrop_pos_high = predrop_pos
predrop_pos_high[2] = current_pose[2]
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos_high,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface_high')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1.fast motion to bin surface
predrop_pos[2] = bin_pose[2] + 0.05
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~2. Guarded move down slow
plan, qf, plan_possible = generatePlan(q_initial,
drop_pose[0:3],
drop_pose[3:7],
tip_hand_transform,
'fast',
guard_on=WeightGuard(binId),
backwards_speed='superSaiyan',
plan_name='guarded_drop')
if plan_possible:
plans_placing.append(plan)
q_initial = qf
else:
return compose_output()
#~3. open gripper
grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
plans_placing.append(grasp_plan)
#~4. open spatula
grasp_plan = EvalPlan('spatula.open()')
plans_placing.append(grasp_plan)
#~5. go to predrop_pos
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos[0:3],
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='predrop_pos')
if plan_possible:
plans_placing.append(plan)
q_initial = qf
else:
return compose_output()
#~6. go to final pose
final_pos = bin_pose[0:3]
final_pos[2] = bin_pose[2] + 0.3
plan, qf, plan_possible = generatePlan(q_initial,
final_pos,
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='final_pose')
if plan_possible:
plans_placing.append(plan)
q_initial = qf
else:
return compose_output()
#~ Execute plans
if plan_possible:
if isExecute:
executePlanForward(plans, withPause)
#We start recording now
lasers.start(binId)
recorder.start_recording(
action='placing',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
rospy.set_param('is_record', True)
executePlanForward(plans_placing, withPause)
rospy.set_param('is_record', False)
recorder.stop_recording(save_action=True)
lasers.stop(binId)
execution_possible = True
return compose_output()
def calibrate_gelsight(isExecute=True, withPause=False):
arc_ori = np.array([0, 1, 0, 0])
ori_1 = np.array([-0.70710678118654757, 0.70710678118654757, 0, 0])
ori_2 = np.array([0.70710678118654757, 0.70710678118654757, 0, 0])
#initialize Parameters
tip_hand_transform = [0, 0, 0, 0, 0, 0]
#read initial robot poses
q_initial = ik.helper.get_joints()
#go to arc_1
# goToHome.goToARC()
plans_list = []
pose_list = []
#arc position (high)
pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0]))
pose_list.append(
np.array([.642, .72005, .95168, .69934, .70796, -0.06744, .07189]))
pose_list.append(
np.array([.642, .72005, .75559, .69934, .70796, -0.06744, .07189]))
pose_list.append(
np.array([.56577, .72005, .75559, .69934, .70796, -0.06744, .07189]))
pose_list.append(
np.array([.48138, .72005, .75559, .69934, .70796, -0.06744, .07189]))
pose_list.append(
np.array([.48138, .72005, .95168, .69934, .70796, -0.06744, .07189]))
pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0]))
speed_list = [
'superSaiyan', 'superSaiyan', 'slow', 'slow', 'slow', 'slow',
'superSaiyan'
]
#gripper hand_commands
# is_gripper_list = [0,0,0,0,0,0,0]
is_gripper_list = [0, 0, 1, 1, 1, 0, 0]
#~1. Open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
plans_list.append(grasp_plan)
#2. generate robot motions
for i in range(len(pose_list)):
print ' speed_list[i]', speed_list[i]
plan, qf, plan_possible = generatePlan(q_initial,
pose_list[i][0:3],
pose_list[i][3:7],
tip_hand_transform,
speed_list[i],
plan_name=str(i))
if plan_possible:
plans_list.append(plan)
q_initial = qf
else:
print('[Release Safe] Plans failed:')
if is_gripper_list[i]:
#sleep
sleep_plan = EvalPlan('rospy.sleep(1.0)')
plans_list.append(sleep_plan)
#close gripper
grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
plans_list.append(grasp_plan)
#sleep
sleep_plan = EvalPlan('rospy.sleep(1.5)')
plans_list.append(sleep_plan)
#capture image
gelsight_plan = EvalPlan('helper.capture_gelsight()')
plans_list.append(gelsight_plan)
#sleep
sleep_plan = EvalPlan('rospy.sleep(.5)')
plans_list.append(sleep_plan)
#open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
plans_list.append(grasp_plan)
#sleep
sleep_plan = EvalPlan('rospy.sleep(2.)')
plans_list.append(sleep_plan)
#execute plan_name
if isExecute and plan_possible:
executePlanForward(plans_list, withPause)
def place(listener,
br,
isExecute=True,
binId=0,
withPause=False,
rel_pose=None,
BoxBody=None,
place_pose=None,
viz_pub=None,
is_drop=True,
recorder=None):
#########################################################
# fhogan and nikhilcd
# Description:
#~2017 picking primitive
#
#~Usage
#~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
#
#~Parameters:
#
#~Inputs:
# 1) objInput: Input from vision or RViz (list of 12, or 7)
# 2) listener: Input from vision or RViz (list of 12, or 7)
# 3) br: Input from vision or RViz (list of 12, or 7)
# 4) isExecute: execute the robot motion if possible or not? (Bool)
# 5) objId: name of object (str)
# 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
# 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
# 8) withPause: Pause between robot motions (Bool)
# 9) rel_pose: object pose relative to link_6
# 10) BoxBody: Bounding box points resolved in link 6 frame
# 11) place_pose: desired position of bounding box
# 12) viz_pub: desired position of bounding box
#
#~Output:
# Dictionary of following keys:
# 1) grasp_possible: Does the object fit in the hand? (True/False)
# 2) plan_possible: Has the plan succeeded? (True/False)
# 3) execution_possible: Is there anything in the grasp? (True/False)
# 4) gripper_opening: gripper opening after grasp attempt (in meters)
# 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
# 6) gelsight_data: [] , dummy for now
#########################################################
#rospy.logdebug(msg, *args)
#rospy.loginfo(msg, *args)
#rospy.logwarn(msg, *args)
#rospy.logerr(msg, *args)
#rospy.logfatal(msg, *args)
###############################
## Pring input variables for ##
###############################
rospy.logdebug('[Picking] withPause %s', withPause)
rospy.logdebug('[Picking] rel_pose %s', rel_pose)
rospy.logdebug('[Picking] BoxBody %s', BoxBody)
rospy.logdebug('[Picking] place_pose %s', place_pose)
########################
## Initialize values ##
########################
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
plans = []
plans_grasping = []
plans_grasping2 = []
plans_guarded = []
plans_guarded2 = []
plans_placing = []
plans_placing2 = []
graspPose = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
liftoff_pub = rospy.Publisher('/liftoff_time',
std_msgs.msg.String,
queue_size=10,
latch=False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPose': graspPose,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
#########################
## Constant parameters ##
#########################
UpdistFromBinFast = 0.15 # Margin above object during "move to a location"
UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location"
gripperOriHome = [0, 1, 0, 0]
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
vision_pos[2] = 0.17786528
#############
## Placing ##
#############
#if primitive called without placing arguments, go to center of bin
if rel_pose == None:
drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
drop_pose[3:7] = gripperOriHome
else:
drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
place_pose, viz_pub,
listener, br)
#~ Adjust height according to weigth
if is_drop:
drop_offset = 0.15
else:
drop_offset = 0.025
#~set drop pose target z position to be bottom of bin
drop_pose[2] = bin_pose[2] - rospy.get_param('tote/height') + drop_offset
#~Predrop: go to top middle bin surface fast without guarded move
predrop_pos = np.array(drop_pose[0:3])
predrop_pos[2] = bin_pose[2] + 0.05
######################
## Generate plans ##
######################
#~move safe to placing bin number
rospy.logdebug(
'[Picking] Collision free placing to binId %s and position %s', binId,
predrop_pos)
# if isExecute:
# q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
# if update_command is not None:
# update_command.execute()
#~0.go up to avoid collision with tote walls
current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
current_pose[2] = current_pose[2] + 0.15
plan, qf, plan_possible = generatePlan(q_initial,
current_pose[0:3],
current_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_up')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~0.1.fast motion to bin surface high
predrop_pos_high = predrop_pos
predrop_pos_high[2] = current_pose[2]
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos_high,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface_high')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1.fast motion to bin surface
predrop_pos[2] = bin_pose[2] + 0.05
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos,
drop_pose[3:7],
tip_hand_transform,
'faster',
plan_name='go_bin_surface')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~2. Guarded move down slow
plan, qf, plan_possible = generatePlan(q_initial,
drop_pose[0:3],
drop_pose[3:7],
tip_hand_transform,
'fast',
guard_on=WeightGuard(binId,
threshold=100),
backwards_speed='superSaiyan',
plan_name='guarded_drop')
if plan_possible:
plans_placing.append(plan)
q_initial = qf
else:
return compose_output()
#~3. open gripper
grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
plans_placing2.append(grasp_plan)
#~4. open spatula
grasp_plan = EvalPlan('spatula.open()')
plans_placing2.append(grasp_plan)
#~5. go to predrop_pos
plan, qf, plan_possible = generatePlan(q_initial,
predrop_pos[0:3],
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='predrop_pos')
if plan_possible:
plans_placing2.append(plan)
q_initial = qf
else:
return compose_output()
#~6. go to final pose
final_pos = bin_pose[0:3]
final_pos[2] = bin_pose[2] + 0.3
plan, qf, plan_possible = generatePlan(q_initial,
final_pos,
drop_pose[3:7],
tip_hand_transform,
'superSaiyan',
plan_name='final_pose')
if plan_possible:
plans_placing2.append(plan)
q_initial = qf
else:
return compose_output()
#~ Execute plans
if plan_possible:
if isExecute:
executePlanForward(plans, withPause)
#We start recording now
if recorder is not None:
lasers.start(binId)
recorder.start_recording(
action='placing',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
rospy.set_param('is_record', True)
executePlanForward(plans_placing, withPause)
if rospy.get_param('is_contact'):
ik.helper.move_cart(dz=0.015)
executePlanForward(plans_placing2, withPause)
rospy.set_param('is_record', False)
if recorder is not None:
recorder.stop_recording(save_action=True)
lasers.stop(binId)
execution_possible = True
return compose_output()
def grasp(objInput,
listener,
br,
isExecute=True,
objId='expo_eraser',
binId=0,
withPause=False,
viz_pub=None,
recorder=None,
is_regrasp=False,
open_hand=True):
#########################################################
# fhogan and nikhilcd
# Description:
#~2017 picking primitive
#
#~Usage
#~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
#
#~Parameters:
#
#~Inputs:
# 1) objInput: Input from vision or RViz (list of 12, or 7)
# 2) listener: Input from vision or RViz (list of 12, or 7)
# 3) br: Input from vision or RViz (list of 12, or 7)
# 4) isExecute: execute the robot motion if possible or not? (Bool)
# 5) objId: name of object (str)
# 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
# 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
# 8) withPause: Pause between robot motions (Bool)
# 9) rel_pose: object pose relative to link_6
# 10) BoxBody: Bounding box points resolved in link 6 frame
# 11) place_pose: desired position of bounding box
# 12) viz_pub: desired position of bounding box
#
#~Output:
# Dictionary of following keys:
# 1) grasp_possible: Does the object fit in the hand? (True/False)
# 2) plan_possible: Has the plan succeeded? (True/False)
# 3) execution_possible: Is there anything in the grasp? (True/False)
# 4) gripper_opening: gripper opening after grasp attempt (in meters)
# 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
# 6) gelsight_data: [] , dummy for now
#########################################################
#rospy.logdebug(msg, *args)
#rospy.loginfo(msg, *args)
#rospy.logwarn(msg, *args)
#rospy.logerr(msg, *args)
#rospy.logfatal(msg, *args)
###############################
## Pring input variables for ##
###############################
rospy.logdebug('[Picking] objInput %s', objInput)
rospy.logdebug('[Picking] objId %s', objId)
rospy.logdebug('[Picking] withPause %s', withPause)
########################
## Initialize values ##
########################
q_initial = ik.helper.get_joints()
bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
spatula_tip_to_tcp_dist = rospy.get_param(
"/gripper/spatula_tip_to_tcp_dist")
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
plans = []
plans_grasping = []
plans_grasping2 = []
plans_guarded = []
plans_guarded2 = []
plans_placing = []
plans_placing2 = []
graspPos = []
plan_possible = False
execution_possible = False
gripper_opening = 0.0
grasp_possible = True
gelsight_data = []
collision = False
final_object_pose = None
rospy.set_param('is_record', False)
rospy.set_param('is_contact', False)
def compose_output():
return {
'collision': collision,
'grasp_possible': grasp_possible,
'plan_possible': plan_possible,
'execution_possible': execution_possible,
'gripper_opening': gripper_opening,
'graspPos': graspPos,
'gelsight_data': gelsight_data,
'final_object_pose': final_object_pose
}
#########################
## Constant parameters ##
#########################
UpdistFromBinFast = 0.15 # Margin above object during "move to a location"
UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location"
gripperOriHome = [0, 1, 0, 0]
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
vision_pos[2] = 0.17786528
###############################
## Picking primitive ##
###############################
# ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]), listener, br, False)
#~Define reference frames
world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
listener=listener, br=br)
#~get grasp pose and gripper opening from vision
if len(objInput) == 12:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
objInput)
if not is_regrasp:
graspPos = graspPos - hand_X * 0.015 * 1
elif len(objInput) == 7:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
objInput, objId, listener, br)
#frank hack for stationary spatula:
grasp_width = 0.11
#~build gripper orientation matrix 3x3
hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
hand_orient_norm = hand_orient_norm.transpose()
#~Grasp relocation script
hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
theta = euler[2] - np.pi
colFreePose = collisionFree(graspPos,
binId=binId,
listener=listener,
br=br,
finger_opening=grasp_width,
safety_margin=0.00,
theta=theta)
graspPos[0:2] = colFreePose[0:2]
#################################
## GENERATE PLANS OF PRIMITIVE ##
#################################.
#~ Start from home position
# if isExecute:
# q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
# scorpion.back()
#~0. Move to a location above the bin, Rotate gripper to grasp orientation
pregrasp_targetPosition = graspPos
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast - 0.1
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_safe_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
#~1. Move to to surface of bin
pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded - 0.1
plan, qf, plan_possible = generatePlan(q_initial,
pregrasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'superSaiyan',
plan_name='go_top_bin')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return compose_output()
if open_hand:
#~2. Open gripper
grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
plans.append(grasp_plan)
#~ 3. Open spatula
grasp_plan = EvalPlan('spatula.open()')
plans.append(grasp_plan)
grasp_targetPosition = deepcopy(graspPos)
# rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)
#~4. sleep
sleep_plan = EvalPlan('rospy.sleep(0.2)')
plans.append(sleep_plan)
#~5. perform guarded move down
grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
'/tote/height') + 0.015 #~frank hack for safety
plan, qf, plan_possible = generatePlan(q_initial,
grasp_targetPosition,
hand_orient_quat,
tip_hand_transform,
'faster',
guard_on=WeightGuard(binId,
threshold=140),
plan_name='guarded_pick')
if plan_possible:
plans_guarded.append(plan)
q_initial = qf
else:
return compose_output()
#~7. Close spatula
grasp_plan = EvalPlan('spatula.close()')
plans_guarded2.append(grasp_plan)
#~8. Close gripper
grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
plans_guarded2.append(grasp_plan)
#~9. sleep
sleep_plan = EvalPlan('rospy.sleep(0.6)')
plans_guarded2.append(sleep_plan)
#~10. Move to a location above the bin
# plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name = 'retrieve_object')
# if plan_possible:
# plans_grasping.append(plan)
# q_initial = qf
# else:
# return compose_output()
################
## EXECUTION ###
################
if isExecute and plan_possible:
executePlanForward(plans, withPause)
###We start recording now
if recorder is not None:
lasers.start(binId)
recorder.start_recording(
action='grasping',
action_id=str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
tote_num=binId,
frame_rate_ratio=5,
image_size=-1)
#Execute guarded move
rospy.set_param('is_record', True)
executePlanForward(plans_guarded, withPause)
if rospy.get_param('is_contact'):
ik.helper.move_cart(dz=0.005)
executePlanForward(plans_guarded2, withPause)
rospy.set_param('is_record', False)
elif not isExecute:
execution_possible = plan_possible
return compose_output()
def push_rotate(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId='crayola_64_ct',
bin_contents=None,
robotConfig=None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute=True,
withPause=True):
#~ *****************************************************************
obj_dim = get_obj_dim(objId)
obj_dim = np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width = 0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.2)
(shelf_position,
shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
shelf_pose_tfm = np.array(shelf_pose_tfm_list)
shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
#~ print 'shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos = shelf_pose_tfm[0:3, 3]
#~ print 'shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm = np.array(obj_pose_tfm_list)
obj_pose_orient = obj_pose_tfm[0:3, 0:3]
obj_pose_pos = obj_pose_tfm[0:3, 3]
#~ print 'obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
#~ print 'obj_X'
#~ print obj_X
obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
#~ print 'obj_Y'
#~ print obj_Y
obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
#~ print 'obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
obj_pose_orient_norm = obj_pose_orient_norm.transpose()
#~ print 'obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
#~ print 'proj'
#~ print proj_vecY
max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir] > 0:
hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
else:
hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]
#~ print 'hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
la.norm(hand_norm_vec))
angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180.0 / np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)
max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir] > 0:
fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
else:
fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]
#~ print 'fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)
max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
np.fabs(proj_vecZ))
Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]
#~ print 'Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis = obj_dim[Zaxis_dir]
hand_Y = np.cross(hand_norm_vec, fing_axis_vec)
#~ *****************************************************************
#################### DECISION STRATEGIES #########################
bin_inner_cnstr = get_bin_inner_cnstr()
proj_handNorm_ShelfX = np.dot(hand_norm_vec, shelf_X)
right_push = False
left_push = False
# print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm
# print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis
#
# print 'proj_handNorm_ShelfX'
# print proj_handNorm_ShelfX
if obj_dim_along_hand_norm > obj_dim_along_fingAxis:
#~ print 'proj_vecY'
#~ print proj_vecY
#~ print proj_vecY[hand_norm_dir]
#~
#~ print 'proj_vecX'
#~ print proj_vecX
#~ print proj_vecX[fing_axis_dir]
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
#~ else:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
if proj_handNorm_ShelfX > 0:
#Do left push
Ypush_mult = 1
left_push = True
print '[PushRotate] Left Push'
else:
#Do right push
Ypush_mult = -1
print '[PushRotate] right Push'
right_push = True
push_offset = (
obj_dim_along_hand_norm / 2.0
) #(Instead pf pushing on edge we will push 5 mm inside)
else:
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
#~ else:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
if proj_handNorm_ShelfX > 0:
#Do right push
Ypush_mult = -1
print '[PushRotate] right Push'
right_push = True
else:
#Do left push
Ypush_mult = 1
print '[PushRotate] Left Push'
left_push = True
push_offset = (obj_dim_along_fingAxis / 2.0)
num_intm_points = 5
push_x_intm = np.zeros(num_intm_points + 1)
push_y_intm = np.zeros(num_intm_points + 1)
backWall_shelf = bin_inner_cnstr[binNum][1]
backWall_world = coordinateFrameTransform([0, backWall_shelf, 0], 'shelf',
'map', listener)
back_check = backWall_world.pose.position.x - obj_dim_along_hand_norm / 2.0
binRightWall = bin_inner_cnstr[binNum][0]
binLeftWall = bin_inner_cnstr[binNum][1]
binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
'shelf', 'map', listener)
binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
'map', listener)
if left_push:
sideWall_check = binRightWall_world.pose.position.y + (
fing_width / 2.0) + (obj_dim_along_fingAxis)
if right_push:
sideWall_check = binLeftWall_world.pose.position.y - (
fing_width / 2.0) - (obj_dim_along_fingAxis)
for i in range(num_intm_points + 1):
push_x_intm[i] = obj_pose_pos[0] + (push_offset) * np.sin(
((90 / num_intm_points) * i * np.pi) / 180.0)
if push_x_intm[i] > back_check:
push_x_intm[i] = back_check
push_y_intm[i] = obj_pose_pos[1] + (Ypush_mult * push_offset) * np.cos(
((90 / num_intm_points) * i * np.pi) / 180.0)
if left_push:
if push_y_intm[i] < sideWall_check:
push_y_intm[i] = sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
if right_push:
if push_y_intm[i] > sideWall_check:
push_y_intm[i] = sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
push_x_intm = np.array(push_x_intm)
#~ print'push_x_intm'
#~ print push_x_intm
push_y_intm = np.array(push_y_intm)
#*******************************************************************
#Move the hand to the center of the bin and open the hand based on the height of the object
bin_mid_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
binFloorHeight_world = coordinateFrameTransform([0, 0, binFloorHeight],
'shelf', 'map', listener)
bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map',
listener)
#*******************************************************************
tcp_Z_off = 0.005
push_tcp_Z_shelfFrame = (
(bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
2.0) - tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform(
[0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z
push_z_intm = (push_tcp_Z) * np.ones(push_x_intm.size)
push_series_pos = np.vstack((push_x_intm, push_y_intm, push_z_intm))
push_series_pos = push_series_pos.transpose()
#~ print 'push_series_pos'
#~ print push_series_pos
#~ print push_series_pos[0,:]
#~ print push_series_pos[-1,:]
push_possible = True
#******************************************************************
fing_push_pt = 0.2 * obj_dim_along_ZAxis + binFloorHeight_world.pose.position.z
blade_tip_TCP_off = 0.038 # dist between finger inner end and spatual edge
push_hand_opening = 2 * (push_tcp_Z - blade_tip_TCP_off - fing_push_pt)
if push_hand_opening > grasp_range_lim:
push_hand_opening = grasp_range_lim
# Spatula finger should not hit the lip of the bin while pushing
lip_Z_shelf = bin_inner_cnstr[binNum][4]
lip_Z_world = coordinateFrameTransform([0, 0, lip_Z_shelf], 'shelf', 'map',
listener)
#~ lip_off=.025
#~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off
max_allowed_hand_opening_out = 2 * (push_tcp_Z -
lip_Z_world.pose.position.z)
max_allowed_hand_opening = max_allowed_hand_opening_out - 0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface
if push_hand_opening > max_allowed_hand_opening:
print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin'
push_hand_opening = max_allowed_hand_opening
#~ print 'push_hand_opening'
#~ print push_hand_opening
#~ #hand should not hit the side walls
#~ binRightWall,binLeftWall=find_shelf_walls(binNum)
#~
#~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
#~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
side_wall_clearance = 0.0
print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y
print '[PushRotate] right_hand_edge=', (push_y_intm[0] - fing_width -
side_wall_clearance)
print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y
print '[PushRotate] left_hand_edge=', (push_y_intm[0] + fing_width +
side_wall_clearance)
if push_y_intm[
0] - fing_width - side_wall_clearance < binRightWall_world.pose.position.y:
print '[PushRotate] Hand will hit the right wall, Push rotate not possible'
push_possible = False
if push_y_intm[
0] + fing_width + side_wall_clearance > binLeftWall_world.pose.position.y:
print '[PushRotate] Hand will hit the left wall, push rotate not possible'
push_possible = False
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient = deepcopy(spatula_down_orient)
#~ *************************************************************
if push_possible == True:
#Now we know where we wan to move TCP
#Let's do robot motion planning now
joint_topic = '/joint_states'
# plan store
plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushRotate] move to bin mouth in push-rotate code successful'
#plan.visualize()
plans.append(plan) # Plan 0
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[PushRotate] move to bin mouth in push-rotate code fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ moveGripper(0.1,100)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0 #0.035
push_l2 = -Ypush_mult * (
fing_width / 2.0
) # This should depend on the righ or left push conditions
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
#~ q_initial = robotConfig
# move just inside the bin with push orientation and open the hand suitable to push
distFromShelf = -0.01
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, push_series_pos[0, 1],
InbinPt.pose.position.z
] #matching world_Y of first push pt
print '[PushRotate] prepush_targetPosition=', prepush_targetPosition
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] move inside bin in push orient successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] move inside bin in push orient fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH ###############
# Open the gripper to dim calculated for pushing
print '[PushRotate] hand opening to'
print push_hand_opening * 1000.0
#~ moveGripper(push_hand_opening,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
#~ push_rotate_traj = Plan()
#~ push_rotate_traj.q_traj = qf
for i in range(0, push_x_intm.size):
pushpt_pos = push_series_pos[i, :].transpose()
pubFrame(br,
pose=pushpt_pos.tolist() + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
#~ pdb.set_trace()
# planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient,
# joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb)
planner = IK(q0=q_initial,
target_tip_pos=pushpt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] point inside push traj successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] point inside push traj fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
#~ if isExecute:
#~ pauseFunc(withPause)
#~ push_rotate_traj.execute()
#################### CLOSE THE GRIPPER GRASP #################
#~ moveGripper(0.0,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize()
pauseFunc(withPause)
plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[len(plans) - numOfPlan - 1].visualizeBackward()
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
#*******************************************************************
print '[PushRotate] push_successful'
return (push_possible, False)