def gentle_move(dir_index, q_initial, pos, gripperOri_list, tip_hand_transform,
listener, point_counter):
plans = []
axis_list = [[0, 1, 0], [-1, 0, 0], [0, -1, 0], [1, 0, 0], [0, 0, -1],
[0, 0, 1]]
axis = axis_list[dir_index]
if point_counter == 4:
bin_counter = 0
elif point_counter == 9:
bin_counter = 1
elif point_counter == 14:
bin_counter = 2
elif point_counter == 19:
bin_counter = 3
for i in range(0, 10000):
for i in range(0, 3):
pos[i] = pos[i] + 0.002 * axis[i]
with Timer('generatePlan'):
plan, qf, plan_possible = generatePlan(q_initial, pos,
gripperOri_list,
tip_hand_transform,
'fastest')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~execute plan
with Timer('executePlanForward'):
executePlanForward(plans, False, False)
value = raw_input(
"Enter: Move 2mm closer to wall, s: Stop robot and record value ")
#save data if s
if value == 's':
fpath = os.environ[
'CODE_BASE'] + '/catkin_ws/src/apc_config/storage_corners.yaml'
stream = open(fpath, 'r')
data = yaml.load(stream)
tcp_pose = get_tcp_pose(listener)
if dir_index == 4:
terms = ['z']
else:
terms = ['x', 'y']
for i in range(0, len(terms)):
if dir_index == 4:
param_name = '/bin' + str(bin_counter) + '/' + terms[i]
value_tmp = tcp_pose[2] - rospy.get_param(
'/gripper/spatula_tip_to_tcp_dist')
else:
param_name = '/corner' + str(
point_counter) + '/' + terms[i]
value_tmp = tcp_pose[i]
value = value_tmp.astype(type('float', (float, ), {}))
data[param_name] = value
with open(fpath, 'w') as yaml_file:
yaml_file.write(yaml.dump(data, default_flow_style=False))
break
else:
plans = []
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 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 unit_test_centers():
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
#~ ~Build hand transform
l1 = 0.0
l2 = 0.0
l3 = rospy.get_param("/gripper/spatula_tip_to_tcp_dist")
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
gripperOriHome = [0, 1, 0, 0]
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
plans = []
withPause = True
#~generate plans
#loop through sections (bins)
plans = []
goToHome.prepGripperPicking()
goToHome.goToARC()
for i in range(0, 9):
binId = i
collision_free_placing(binId, listener, isSuction=False)
#~READ robot position
rospy.sleep(.3)
tcpPose = get_tcp_pose(listener, tcp_offset=l3)
tcpPose[2] = rospy.get_param(
'/bin' + str(i) +
'_pose/z') #~- rospy.get_param('/bin'+str(i)+'/height')
rospy.sleep(.3)
q_initial = get_joints()
rospy.sleep(1.0)
##move to edge of structure
map_bin_id_to_ws_id = {
0: 0,
1: 1,
2: 2,
3: 3,
4: 0,
5: 0,
6: 4,
7: 4,
8: 5
}
plan, qf, plan_possible = generatePlan(q_initial, tcpPose[0:3],
gripperOriHome,
tip_hand_transform, 'fast')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
##move to edge of structure
map_bin_id_to_ws_id = {
0: 0,
1: 1,
2: 2,
3: 3,
4: 0,
5: 0,
6: 4,
7: 4,
8: 5
}
plan, qf, plan_possible = generatePlan(
q_initial,
tcpPose[0:3] - np.array([0, 0, 0.2]),
gripperOriHome,
tip_hand_transform,
'fast',
guard_on=WeightGuard(map_bin_id_to_ws_id[binId], threshold=100))
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
executePlanForward(plans, False)
if binId > 5:
collision_free_placing(binId, listener, isSuction=False)
plans = []
def unit_test_collision(flag=0):
#~ ~Build hand transform
l1 = 0.0
l2 = 0.0
l3 = rospy.get_param("/gripper/calibration_to_tcp_dist")
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
gripperOriHome = [0, 1, 0, 0]
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
plans = []
withPause = True
#~generate plans
#loop through sections (bins)
counter = 0
section_points_list = [[0, 1, 2, 3], [0, 1, 2, 3], [0, 1, 2, 3], [0, 2, 3]]
#~bin corners
for i in range(0, 4):
#~get corner points from yaml
points_warped_list, points_list = get_bin_corners(i)
points_warped = np.asarray(points_warped_list)
points = np.asarray(points_list)
box1 = points
box2 = points_warped
####################
## middle point ##
###################
point_middle_list = get_bin_middle(i)
point_middle = np.asarray(point_middle_list[0])
#~transform to warped frame
point_middle_warped = rect2quad(point_middle, box1, box2)
#~ point_middle_warped = quad2quad(point_middle, box1, box2)
#~ point_middle_warped = interp2d(point_middle, box1, box2)
target_middle = np.hstack((point_middle_warped, 0.05))
####################
## corner points ##
###################
for term in section_points_list[i]:
##move to middle
plan, qf, plan_possible = generatePlan(q_initial, target_middle,
gripperOriHome,
tip_hand_transform,
'fastest')
counter += 1
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~ target_position2d_warped = np.array(points_warped_list[term])
target_position2d = np.array(points_list[term])
#~convert to warped frame
target_position2d = rect2quad(target_position2d, box1, box2)
#~ target_position2d = quad2quad(target_position2d,box1,box2)
#~ target_position2d = interp2d(target_position2d,box1,box2)
target_position = np.hstack((target_position2d, 0.05))
##move to edge of structure
target_middle2 = np.array(
[target_middle[0], target_position[1], target_middle[2]])
##move to middle edge
plan, qf, plan_possible = generatePlan(q_initial, target_middle2,
gripperOriHome,
tip_hand_transform,
'fastest')
counter += 1
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~generate plan
plan, qf, plan_possible = generatePlan(q_initial, target_position,
gripperOriHome,
tip_hand_transform,
'fastest')
counter += 1
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
##move to edge of structure
plan, qf, plan_possible = generatePlan(q_initial, target_middle2,
gripperOriHome,
tip_hand_transform,
'fastest')
counter += 1
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~Execute plans
goToARC()
executePlanForward(plans, withPause)
def calibrate_storage():
#################################################
#~ Generate Plans
gripperOri = []
gripperOri.append([-1., 0., 0., 0.])
gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.])
gripperOri.append([0., -1., 0., 0.])
gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.])
#~build total list plan
gripperOri_list = []
ori_list = [
0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
0, 0, 1, 2, 3, 0, 0, 0
]
for i in range(0, len(ori_list)):
gripperOri_list.append(gripperOri[ori_list[i]])
#~positions
pos = []
#~bin0
bin_pose = get_params_yaml('bin0_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, -0.46 + 0.19647, 0.41])
pos.append([0.75 + .12, bin_pose[1], 0.41])
pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41])
pos.append([1.19 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append(
[1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin1
bin_pose = get_params_yaml('bin1_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41])
pos.append([0.75 + .12, bin_pose[1], 0.41])
pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41])
pos.append([1.19 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([
0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05
])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin2
bin_pose = get_params_yaml('bin2_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41])
pos.append([0.8 + .12, bin_pose[1], 0.41])
pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41])
pos.append([1.18 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append(
[0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
####################
## Generate Plans ##
####################
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
tip_hand_transform = [0., 0., 0., 0., 0., 0.]
dir_list = [
None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
0, 1, 2, 3, None
]
point_counter = 0
#~generate plans
plans = []
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.5)
goToHome.goToARC()
#~ goToHome.prepGripperPicking()
for i in range(0, len(pos)):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i],
tip_hand_transform, listener, point_counter)
point_counter += 1
def calibrate_boxes():
#################################################
#~ Generate Plans
gripperOri = []
gripperOri.append([-1., 0., 0., 0.])
gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.])
gripperOri.append([0., -1., 0., 0.])
gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.])
#~build total list plan
gripperOri_list = []
ori_list = [
0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0
]
for i in range(0, len(ori_list)):
gripperOri_list.append(gripperOri[ori_list[i]])
#~positions
pos = []
#~bin4
bin_pose = get_params_yaml('bin4_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.1, -0.498443692923, 0.5512911677361 - .15])
pos.append([1.05, -0.599031984806, 0.5512911677361 - .15])
pos.append([1.14, -0.70, 0.5512911677361 - .15])
pos.append([1.2, -0.618239402771, 0.5512911677361 - .15])
pos.append([
bin_pose[0], bin_pose[1],
0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin5
bin_pose = get_params_yaml('bin5_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15])
pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15])
pos.append([0.79457449913, -0.70, 0.5512911677361 - .15])
pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin6
bin_pose = get_params_yaml('bin6_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0])
pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0])
pos.append([0.099, -0.68, 0.539945185184 + 0.0])
pos.append([0.15, -0.6, 0.539945185184 + 0.0])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.145160362124, -0.846666872501, 0.65])
pos.append([
bin_pose[0], bin_pose[1],
0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin7
bin_pose = get_params_yaml('bin7_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0])
pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0])
pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0])
pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.203513264656, -0.86, 0.65])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin8
bin_pose = get_params_yaml('bin8_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.09, 0.68, 0.716840863228])
pos.append([-0.22, 0.62, 0.716840863228])
pos.append([-0.09, 0.5, 0.716840863228])
pos.append([0.1, 0.62, 0.716840863228])
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.106256209314, 0.78, 0.8])
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
####################
## Generate Plans ##
####################
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
tip_hand_transform = [0., 0., 0., 0., 0., 0.]
dir_list = [
None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1,
2, 3, None, 4, None
]
point_counter = 0
#~generate plans
plans = []
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.5)
goToHome.goToARC()
goToHome.prepGripperPicking()
###############################
## Unwrap angles example use ##
###############################
#~1) function to unwarp an angle to the range [-pi,pi]
def unwrap(angle):
if angle > np.pi:
angle = np.mod(angle, 2 * np.pi)
if angle > np.pi:
angle = angle - 2 * np.pi
elif angle < -np.pi:
angle = np.mod(angle, -2 * np.pi)
if angle < -np.pi:
angle = angle + 2 * np.pi
return angle
for i in range(0, 16):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
#~2) unwrap angle of joint 6 only
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
#~3) set q_initial (for next plan) with the unwrapped angle joint state
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
##################################
plans_col_free = []
plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
q_initial, plans_col_free, [0, 1, 0, 0], False, 6)
executePlanForward(plans_col_free, True)
for i in range(16, 32):
#~ for i in range(0,16):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
plans_col_free = []
plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
q_initial, plans_col_free, [0, 1, 0, 0], False, 8)
executePlanForward(plans_col_free, True)
for i in range(32, 40):
#~ for i in range(16,24):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
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 retrieve(listener,
br,
isExecute=True,
objId='expo_eraser',
binId=0,
withPause=False,
viz_pub=None,
recorder=None,
ws_object=None,
isShake=True):
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
}
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")
l1 = 0.0
l2 = 0.0
l3 = spatula_tip_to_tcp_dist
tip_hand_transform = [l1, l2, l3, 0, 0, 0]
delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
delta_vision_pose[3:7] = np.array([0, 1, 0, 0])
vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
vision_pos[2] = 0.17786528 - 0.1
plans = []
plans_grasping = []
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)
# ~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()
if isExecute and plan_possible:
#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)
#~Check if picking success
low_threshold = 0.0035
high_threshold = 0.027
#shake robot
rospy.sleep(1.5)
if isShake and gripper.getGripperopening() > high_threshold and isExecute:
# We initialize drop listener
ws_object.start_listening_for_drop(bin_num=binId)
ik.ik.shake()
rospy.sleep(2.)
# We stop drop listener
ws_object.stop_listening_for_drop()
### We stop recording
if recorder is not None:
recorder.stop_recording(save_action=True)
lasers.stop(binId)
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 #temporary hack
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()
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()