def percept(obj_pose=None,
binNum=0,
obj_id=None,
bin_contents=None,
isExecute=True,
withPause=True):
capsen.capsen.init()
print '[Percept] percept heuristic'
goToHome.goToHome(robotConfig=None,
homePos=[1, 0, 1.2],
isExecute=isExecute,
withPause=withPause)
kinect_obj_pose = None
if useKinect:
print '[Percept] kinect percept'
kinect_obj_pose, kinect_score = capsen.capsen.detectOneObject(
obj_id, bin_contents, binNum, mode=0, withScore=True)
if kinect_obj_pose is None:
print '[Percept] kinect percept failed.'
else:
kinect_pose_type = find_object_pose_type(obj_id, kinect_obj_pose)
print '[Percept] kinect percept success.'
if not (objectUseHandVisionList[obj_id]
or binNum in binUseHandVisionList or kinect_obj_pose is None):
return kinect_obj_pose, kinect_pose_type
# turn off kinect
kinect_stop()
print '[Percept] Perception heuristic wants to do percept_hand.'
hand_obj_pose, hand_pose_type, hand_score = percept_hand(
obj_pose, binNum, obj_id, bin_contents, isExecute, withPause)
# turn on kinect
kinect_start()
if hand_obj_pose and kinect_obj_pose:
if hand_score >= kinect_score:
return hand_obj_pose, hand_pose_type
else:
return kinect_obj_pose, kinect_pose_type
elif hand_obj_pose:
return hand_obj_pose, hand_pose_type
elif kinect_obj_pose:
return kinect_obj_pose, kinect_pose_type
else:
return None, None
def percept(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True):
capsen.capsen.init()
print '[Percept] percept heuristic'
goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
kinect_obj_pose = None
if useKinect:
print '[Percept] kinect percept'
kinect_obj_pose, kinect_score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 0, withScore = True)
if kinect_obj_pose is None:
print '[Percept] kinect percept failed.'
else:
kinect_pose_type = find_object_pose_type(obj_id, kinect_obj_pose)
print '[Percept] kinect percept success.'
if not (objectUseHandVisionList[obj_id] or binNum in binUseHandVisionList or kinect_obj_pose is None):
return kinect_obj_pose, kinect_pose_type
# turn off kinect
kinect_stop()
print '[Percept] Perception heuristic wants to do percept_hand.'
hand_obj_pose, hand_pose_type, hand_score = percept_hand(obj_pose, binNum, obj_id, bin_contents, isExecute, withPause)
# turn on kinect
kinect_start()
if hand_obj_pose and kinect_obj_pose:
if hand_score >= kinect_score:
return hand_obj_pose, hand_pose_type
else:
return kinect_obj_pose, kinect_pose_type
elif hand_obj_pose:
return hand_obj_pose, hand_pose_type
elif kinect_obj_pose:
return kinect_obj_pose, kinect_pose_type
else:
return None, None
def manual_cal_2(binNum=0, endBinNum=11):
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [
l1, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
moveDistVertical = 0.08 #we could do 1 more cm
#set Speed
ik.ik.setSpeedByName('faster')
# broadcast frame attached to tcp
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0,
0.7071] # set topple orientation (rotate wrist)
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
print 'TCP is at:', tcpPos
print 'planning to go home'
# home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
# planner = IKJoint(target_joint_pos=home_joint_pose)
# plan = planner.plan()
# plan.visualize()
raw_input('execute?')
# plan.execute()
goToHome.goToHome()
print 'done going home'
calibBins = []
for x in range(binNum, endBinNum + 1):
calibPts = []
if x == 1 or x == 4 or x == 7 or x == 10:
calibBins.append([])
continue
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
distFromShelf, x)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
listener)
mouthBin = [
mouthBin.pose.position.x, mouthBin.pose.position.y,
mouthBin.pose.position.z
]
epsilon = 0.016
distFromShelf = distFromShelf - epsilon
# go to mouth of bin
print 'planning to go to mouth but a little out'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go to the mouth of the bin but actually inside
print 'planning to go to start position'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2]]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to start position'
OrientationList = []
targetPtList = []
captionList = []
ntouch = 5
# go down
captionList.append('go down')
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1],
mouthBin[2] - moveDistVertical
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go left
captionList.append('go left')
Orientation = [0.5, 0.5, 0.5, 0.5]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1] + moveDistHorizontal,
mouthBin[2]
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go right
captionList.append('go right')
Orientation = [0.5, -0.5, 0.5, -0.5]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1] - moveDistHorizontal,
mouthBin[2]
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go up
captionList.append('go up')
Orientation = [0.7071, 0, 0.7071, 0]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1],
mouthBin[2] + moveDistVertical
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go to mid back of the bin
captionList.append('go mid back')
Orientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
targetPt = [mouthBin[0] + 0.30, mouthBin[1], mouthBin[2] - 0.08]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
for touchInd in range(ntouch):
print captionList[touchInd]
targetPt = targetPtList[touchInd]
Orientation = OrientationList[touchInd]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going down, TELEOP TIME:'
raw_input('when done with teleop hit enter: ')
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
t = listener.getLatestCommonTime('calib', 'map')
(calibPoseTrans,
calibPoseRot) = listener.lookupTransform('map', 'calib', t)
calibPose = list(calibPoseTrans) + list(calibPoseRot)
#~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
#~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
calibPts.append(calibPose)
raw_input('done recording position, hit enter to go back')
if s:
plan.executeBackward()
else:
print 'Please teleop back'
raw_input('done?')
calibBins.append(calibPts)
filename = os.environ[
'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
with open(filename, 'w') as outfile:
json.dump(calibBins, outfile)
print 'planning to go home'
home_joint_pose = [
-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715,
0.1061231983, -0.1045031463
]
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('faster')
plan.execute()
print 'done going home'
print 'dumped to file ', filename
if __name__=='__main__':
#scoop()
rospy.init_node('listener', anonymous=True)
suction.stop()
objPoses = []
objPoses.append([1.599, 0.450, 1.106, -0.0030, 0.6979, -0.7161, 0.0023]) # upper left corner
objPoses.append([1.607, -0.494, 1.130, -0.0030, 0.6979, -0.7161, 0.0023]) # uppper right corner
objPoses.append([1.598, 0.467, 0.412, -0.0030, 0.6911, -0.7227, 0.0023]) # lower left corner
objPoses.append([1.660, -0.500, 0.379, -0.0384, 0.6970, -0.7150, 0.0368]) #lower right corner
binNums = [0,2,9,11]
goToHome.goToHome()
for x in range(0,4):
suction_side(
objPose = objPoses[x],
binNum = binNums[x],
objId='expo_dry_erase_board_eraser' ,
robotConfig=None,
shelfPosition = [1.9019,0.00030975,-0.503],
isExecute = True,
withPause = False)
goToHome.goToHome()
# posesrv = rospy.ServiceProxy('/pose_service', GetPose) # should move service name out
# rospy.sleep(0.5)
# data = posesrv('','')
def jsonTest(heuristic,
strategies,
jsonFilename,
useVision=False,
withPause=False,
isExecute=True,
toSort=False,
forceSucc=False):
globalSpeedSet = 'yolo'
ik.ik.setSpeedByName(globalSpeedSet)
objectiveBinPos = [1.15, 0, 0.2]
import json
from pprint import pprint
with open(jsonFilename) as data_file:
data = json.load(data_file)
if useVision:
capsen.capsen.init()
bin_contents_all = data['bin_contents']
work_order = data['work_order']
if toSort:
work_order = sortOrder(work_order, bin_contents_all)
displayOrder(work_order, bin_contents_all)
goToHome.goToHome(robotConfig=None,
homePos=[1, 0, 1.2],
isExecute=isExecute,
withPause=withPause)
count_suck = 0
count_grasp = 0
for orderInd, order in enumerate(work_order):
displayOrder(work_order, bin_contents_all, orderInd)
ik.ik.setSpeedByName(globalSpeedSet)
count_try = 0
try_max = 10
isDirty = False
bin_id = order['bin'] # e.g. bin_A
binNum = bin_id2num(bin_id) # e.g. 0
obj_id = order['item'] # object's id in string
print '\n%s\n[Heuristic] [Order %d] bin_id:' % (
'-' * 70,
orderInd), bin_id, binNum, '; obj_id:', obj_id, '\n%s' % ('-' * 70)
if useVision:
try:
obj_pose, pose_type = percept.percept(
obj_id=obj_id,
binNum=binNum,
bin_contents=bin_contents_all[bin_id],
isExecute=isExecute,
withPause=withPause)
if obj_pose is None:
print '[Heuristic] vision failed, continue to next order.'
continue
except:
print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc(
)
continue
else:
obj_pose, pose_type = percept.perceptFromManualFit(obj_id)
print '[Heuristic] pose_type:', pose_type, '; pose:', map(
shortfloat, obj_pose)
resetStrat = True
tryFlag = True
visionSucc = True
while resetStrat and tryFlag and visionSucc:
resetStrat = False
strategy_list = heuristic[obj_id][pose_type]
for i in range(len(strategy_list)):
strategy_name = strategy_list[i]
print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name
count_try += 1
if count_try == try_max:
print '[Heuristic] reached maximum trial number'
tryFlag = False
break
if strategy_name == 'percept' or strategy_name == 'percept-hand':
if isDirty:
print '[Heuristic] perception has been called'
if useVision:
ik.ik.setSpeedByName(globalSpeedSet)
obj_pose, pose_type_new = percept.percept(
obj_pose=obj_pose,
binNum=binNum,
obj_id=obj_id,
bin_contents=bin_contents_all[bin_id],
isExecute=isExecute,
withPause=withPause)
else:
obj_pose, pose_type_new = percept.perceptFromManualFit(
obj_id)
if obj_pose is None:
visionSucc = False
break
else:
isDirty = False
print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose
if pose_type_new == pose_type:
pose_type = pose_type_new
continue
else:
pose_type = pose_type_new
resetStrat = True
break
else:
try:
if strategy_name in strategies:
goToMouth.goToMouth(robotConfig=None,
binNum=binNum,
isExecute=True,
withPause=withPause)
(isDirty, succ) = strategies[strategy_name](
obj_pose,
binNum,
obj_id,
bin_contents_all[bin_id],
isExecute=isExecute,
withPause=withPause) #run it
if forceSucc and (i == len(strategy_list) - 1
or count_try == try_max - 1
): ###### for testing in virtual
succ = True
else:
print '[Heuristic] Strategy', strategy_name, 'not implemented yet. Skip it.'
continue
if succ:
print '[Heuristic] Strategy', strategy_name, 'success'
ik.ik.setSpeedByName('faster')
print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name
if strategy_name == 'suck-down' or strategy_name == 'suck-side':
count_suck = count_suck + 1
attemptGoToBin = goToBin.goToBin(
robotConfig=None,
objectiveBinPos=objectiveBinPos,
isExecute=isExecute,
withPause=withPause,
withSuction=True,
counter=count_suck)
else:
count_grasp = count_grasp + 1
attemptGoToBin = goToBin.goToBin(
binNum=binNum,
robotConfig=None,
objectiveBinPos=objectiveBinPos,
isExecute=isExecute,
withPause=withPause,
withSuction=False,
counter=count_grasp)
if not attemptGoToBin:
goToHome.goToHome(robotConfig=None,
homePos=[1, 0, 1.2],
isExecute=isExecute,
withPause=withPause)
goToBin.goToBin(
binNum=binNum,
robotConfig=None,
objectiveBinPos=objectiveBinPos,
isExecute=isExecute,
withPause=withPause,
withSuction=False,
counter=count_grasp)
break
except:
print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc(
)
if withPause:
raw_input(
'[Heuristic] Press any key to continue next strategy.'
)
ik.ik.setSpeedByName(globalSpeedSet)
goToHome.goToHome(robotConfig=None,
homePos=[1, 0, 1.2],
isExecute=isExecute,
withPause=withPause)
if withPause:
raw_input('[Heuristic] Press any key to continue next object.')
print '\n[Heuristic] We are done!'
def jsonTest(heuristic, strategies, jsonFilename, useVision = False, withPause = False, isExecute = True, toSort = False, forceSucc = False):
globalSpeedSet = 'yolo'
ik.ik.setSpeedByName(globalSpeedSet)
objectiveBinPos = [1.15, 0 , 0.2]
import json
from pprint import pprint
with open(jsonFilename) as data_file:
data = json.load(data_file)
if useVision:
capsen.capsen.init()
bin_contents_all = data['bin_contents']
work_order = data['work_order']
if toSort:
work_order = sortOrder(work_order, bin_contents_all)
displayOrder(work_order, bin_contents_all)
goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
count_suck = 0
count_grasp = 0
for orderInd, order in enumerate(work_order):
displayOrder(work_order, bin_contents_all, orderInd)
ik.ik.setSpeedByName(globalSpeedSet)
count_try = 0
try_max = 10
isDirty = False
bin_id = order['bin'] # e.g. bin_A
binNum = bin_id2num(bin_id) # e.g. 0
obj_id = order['item'] # object's id in string
print '\n%s\n[Heuristic] [Order %d] bin_id:' % ('-'*70, orderInd), bin_id, binNum, '; obj_id:', obj_id , '\n%s' % ('-'*70)
if useVision:
try:
obj_pose, pose_type = percept.percept(obj_id=obj_id, binNum=binNum, bin_contents = bin_contents_all[bin_id], isExecute = isExecute, withPause = withPause)
if obj_pose is None:
print '[Heuristic] vision failed, continue to next order.'
continue
except:
print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc()
continue
else:
obj_pose, pose_type = percept.perceptFromManualFit(obj_id)
print '[Heuristic] pose_type:', pose_type, '; pose:', map(shortfloat, obj_pose)
resetStrat = True
tryFlag = True
visionSucc = True
while resetStrat and tryFlag and visionSucc:
resetStrat = False
strategy_list = heuristic[obj_id][pose_type]
for i in range(len(strategy_list)):
strategy_name = strategy_list[i]
print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name
count_try += 1
if count_try == try_max:
print '[Heuristic] reached maximum trial number'
tryFlag = False
break
if strategy_name == 'percept' or strategy_name == 'percept-hand':
if isDirty:
print '[Heuristic] perception has been called'
if useVision:
ik.ik.setSpeedByName(globalSpeedSet)
obj_pose, pose_type_new = percept.percept(obj_pose = obj_pose, binNum = binNum, obj_id = obj_id, bin_contents = bin_contents_all[bin_id],
isExecute = isExecute, withPause = withPause)
else:
obj_pose, pose_type_new = percept.perceptFromManualFit(obj_id)
if obj_pose is None:
visionSucc = False
break
else:
isDirty = False
print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose
if pose_type_new == pose_type:
pose_type = pose_type_new
continue
else:
pose_type = pose_type_new
resetStrat = True
break
else:
try:
if strategy_name in strategies:
goToMouth.goToMouth(robotConfig=None, binNum = binNum, isExecute = True, withPause = withPause)
(isDirty,succ) = strategies[ strategy_name ](obj_pose, binNum, obj_id, bin_contents_all[bin_id],
isExecute = isExecute, withPause = withPause) #run it
if forceSucc and (i == len(strategy_list)-1 or count_try == try_max-1): ###### for testing in virtual
succ = True
else:
print '[Heuristic] Strategy', strategy_name , 'not implemented yet. Skip it.'
continue
if succ:
print '[Heuristic] Strategy', strategy_name , 'success'
ik.ik.setSpeedByName('faster')
print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name
if strategy_name == 'suck-down' or strategy_name == 'suck-side':
count_suck = count_suck + 1
attemptGoToBin = goToBin.goToBin( robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute,
withPause = withPause, withSuction = True, counter = count_suck)
else:
count_grasp = count_grasp + 1
attemptGoToBin = goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute,
withPause = withPause, withSuction = False, counter = count_grasp)
if not attemptGoToBin:
goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute,
withPause = withPause, withSuction = False, counter = count_grasp)
break
except:
print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc()
if withPause: raw_input('[Heuristic] Press any key to continue next strategy.')
ik.ik.setSpeedByName(globalSpeedSet)
goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
if withPause: raw_input('[Heuristic] Press any key to continue next object.')
print '\n[Heuristic] We are done!'
suction.stop()
objPoses = []
objPoses.append([1.599, 0.450, 1.106, -0.0030, 0.6979, -0.7161,
0.0023]) # upper left corner
objPoses.append([1.607, -0.494, 1.130, -0.0030, 0.6979, -0.7161,
0.0023]) # uppper right corner
objPoses.append([1.598, 0.467, 0.412, -0.0030, 0.6911, -0.7227,
0.0023]) # lower left corner
objPoses.append([1.660, -0.500, 0.379, -0.0384, 0.6970, -0.7150,
0.0368]) #lower right corner
binNums = [0, 2, 9, 11]
goToHome.goToHome()
for x in range(0, 4):
suction_down(objPose=objPoses[x],
binNum=binNums[x],
objId='expo_dry_erase_board_eraser',
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
isExecute=True,
withPause=True)
goToHome.goToHome()
# posesrv = rospy.ServiceProxy('/pose_service', GetPose) # should move service name out
# rospy.sleep(0.5)
# data = posesrv('','')
# pos_x= data.pa.object_list[0].pose.position.x
# pos_y= data.pa.object_list[0].pose.position.y
import goToHome
temp_bias = 0.04 #TO-DO: Remove this bias, calculate object height correctly
topple.topple(objPose = [1.55620419979, 0.281148612499, 1.14214038849-temp_bias,0,0,0,0],
objId = 0,
binNum=0,
robotConfig=None,
shelfPosition = [1.9019,0.00030975,-0.503],
forceThreshold = 1,
effect = 'topple',
binDepthReach = 0.37,
isExecute = True,
withPause = False)
scoop.scoop(
objPose = [1.55620419979, 0.281148612499, 1.14214038849,0,0,0,0],
binNum=0,
robotConfig=None,
shelfPosition = [1.9019,0.00030975,-0.503],
isExecute = True,
withPause = False)
goToBin.goToBin(robotConfig=None,
objectiveBinPos = [0.9,0,0],
isExecute = True,
withPause = False)
goToHome.goToHome(robotConfig = None,
homePos = [1,0,1.2],
isExecute = True,
withPause = False)
def manual_cal_2(binNum = 0, endBinNum = 11):
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
moveDistVertical = 0.08 #we could do 1 more cm
#set Speed
ik.ik.setSpeedByName('faster')
# broadcast frame attached to tcp
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
print 'TCP is at:', tcpPos
print 'planning to go home'
# home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
# planner = IKJoint(target_joint_pos=home_joint_pose)
# plan = planner.plan()
# plan.visualize()
raw_input('execute?')
# plan.execute()
goToHome.goToHome()
print 'done going home'
calibBins = []
for x in range(binNum, endBinNum+1):
calibPts = []
if x == 1 or x == 4 or x == 7 or x == 10:
calibBins.append([])
continue
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, x)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
epsilon = 0.016
distFromShelf = distFromShelf - epsilon
# go to mouth of bin
print 'planning to go to mouth but a little out'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go to the mouth of the bin but actually inside
print 'planning to go to start position'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to start position'
OrientationList = []
targetPtList = []
captionList = []
ntouch = 5
# go down
captionList.append('go down')
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]-moveDistVertical]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go left
captionList.append('go left')
Orientation = [0.5, 0.5, 0.5, 0.5]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]+moveDistHorizontal, mouthBin[2]]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go right
captionList.append('go right')
Orientation = [0.5, -0.5, 0.5, -0.5]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]-moveDistHorizontal, mouthBin[2]]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go up
captionList.append('go up')
Orientation = [0.7071, 0, 0.7071, 0]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]+moveDistVertical]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go to mid back of the bin
captionList.append('go mid back')
Orientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
targetPt = [mouthBin[0]+0.30, mouthBin[1], mouthBin[2]-0.08]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
for touchInd in range(ntouch):
print captionList[touchInd]
targetPt = targetPtList[touchInd]
Orientation = OrientationList[touchInd]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going down, TELEOP TIME:'
raw_input('when done with teleop hit enter: ')
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
t = listener.getLatestCommonTime('calib', 'map')
(calibPoseTrans, calibPoseRot) = listener.lookupTransform('map','calib',t)
calibPose = list(calibPoseTrans) + list(calibPoseRot)
#~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
#~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
calibPts.append(calibPose)
raw_input('done recording position, hit enter to go back')
if s:
plan.executeBackward()
else:
print 'Please teleop back'
raw_input('done?')
calibBins.append(calibPts)
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
with open(filename, 'w') as outfile:
json.dump(calibBins, outfile)
print 'planning to go home'
home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('faster')
plan.execute()
print 'done going home'
print 'dumped to file ', filename
def percept_hand(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True):
capsen.capsen.init()
home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
planner = IKJoint(target_joint_pos=home_joint_pose);
#plan = Plan()
#plan.q_traj = [home_joint_pose];
plan = planner.plan()
plan.visualize();
plan.setSpeedByName('yolo');
if withPause:
print '[Percept] Going back to percept_hand home'
pause()
plan.execute()
bin_cnstr = get_bin_inner_cnstr()
# 1. init target poses
target_poses = []
refind = 7
for i in range(12):
target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist()
target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist()
if i < 3:
target_pose = copy.deepcopy(target_pose_7_invhori)
if i == 0:
target_pose[1] -= 0.06
target_pose[2] -= 0.08 # z
elif i<6:
target_pose = copy.deepcopy(target_pose_7_hori)
target_pose[0] += 0.03 # x
target_pose[2] -= 0.04 # z
elif i<9:
target_pose = copy.deepcopy(target_pose_7_hori)
else:
target_pose = copy.deepcopy(target_pose_7_hori)
target_pose[2] += 0.1 # z
target_pose[1] += (bin_cnstr[i][0]+bin_cnstr[i][1])/2 - (bin_cnstr[refind][0]+bin_cnstr[refind][1])/2 # y
target_pose[2] += (bin_cnstr[i][4]+bin_cnstr[i][5])/2 - (bin_cnstr[refind][4]+bin_cnstr[refind][5])/2 # z
target_poses.append(target_pose)
y_deltas = [0.08, -0.08]
caption = ['left', 'right']
max_score = -100
max_obj_pose = None
for i in range(len(y_deltas)):
# 2. plan to target
tip_hand_transform = xyzrpy_from_xyzquat([-0.067, 0.027, -0.012, -0.007, 0.704, 0.710, -0.002]) # need to be updated
target_pose = copy.deepcopy(target_poses[binNum])
target_pose[1] += y_deltas[i]
plan = None
planner = IK(target_tip_pos = target_pose[0:3], target_tip_ori = target_pose[3:7],
tip_hand_transform=tip_hand_transform,
target_link='link_5', target_joint_bb=[[6, 1.2, 1.2]])
for j in range(15):
planner.ori_tol = 0.5*j/10.0
planner.pos_tol = 0.01*j/10.0
newplan = planner.plan()
if newplan.success():
plan = newplan
print '[Percept] hand planning success at the trial %d' % j
break
if plan:
plan.setSpeedByName('fastest')
plan.visualize()
if withPause:
print '[Percept] Going to percept_hand %s ' % caption[i]
pause()
plan.execute()
else:
print '[Percept] hand planning failed'
continue
# 3. percept
rospy.sleep(0.8) # wait for new pointcloud
obj_pose, score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 1, withScore = True)
if score > max_score:
max_obj_pose = obj_pose
max_score = score
# 4. move back to percept home
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize();
plan.setSpeedByName('yolo');
if withPause:
print '[Percept] Going back to percept_hand home'
pause()
plan.execute()
goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
if max_obj_pose is None:
return None, None, None
pose_type = find_object_pose_type(obj_id, max_obj_pose)
return max_obj_pose, pose_type, max_score
def percept_hand(obj_pose=None,
binNum=0,
obj_id=None,
bin_contents=None,
isExecute=True,
withPause=True):
capsen.capsen.init()
home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
planner = IKJoint(target_joint_pos=home_joint_pose)
#plan = Plan()
#plan.q_traj = [home_joint_pose];
plan = planner.plan()
plan.visualize()
plan.setSpeedByName('yolo')
if withPause:
print '[Percept] Going back to percept_hand home'
pause()
plan.execute()
bin_cnstr = get_bin_inner_cnstr()
# 1. init target poses
target_poses = []
refind = 7
for i in range(12):
target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
2.2, 0, math.pi / 2).tolist()
target_pose_7_invhori = [1.1756, 0, 0.79966
] + tfm.quaternion_from_euler(
-math.pi / 2, 0, -math.pi / 2).tolist()
if i < 3:
target_pose = copy.deepcopy(target_pose_7_invhori)
if i == 0:
target_pose[1] -= 0.06
target_pose[2] -= 0.08 # z
elif i < 6:
target_pose = copy.deepcopy(target_pose_7_hori)
target_pose[0] += 0.03 # x
target_pose[2] -= 0.04 # z
elif i < 9:
target_pose = copy.deepcopy(target_pose_7_hori)
else:
target_pose = copy.deepcopy(target_pose_7_hori)
target_pose[2] += 0.1 # z
target_pose[1] += (bin_cnstr[i][0] + bin_cnstr[i][1]) / 2 - (
bin_cnstr[refind][0] + bin_cnstr[refind][1]) / 2 # y
target_pose[2] += (bin_cnstr[i][4] + bin_cnstr[i][5]) / 2 - (
bin_cnstr[refind][4] + bin_cnstr[refind][5]) / 2 # z
target_poses.append(target_pose)
y_deltas = [0.08, -0.08]
caption = ['left', 'right']
max_score = -100
max_obj_pose = None
for i in range(len(y_deltas)):
# 2. plan to target
tip_hand_transform = xyzrpy_from_xyzquat(
[-0.067, 0.027, -0.012, -0.007, 0.704, 0.710,
-0.002]) # need to be updated
target_pose = copy.deepcopy(target_poses[binNum])
target_pose[1] += y_deltas[i]
plan = None
planner = IK(target_tip_pos=target_pose[0:3],
target_tip_ori=target_pose[3:7],
tip_hand_transform=tip_hand_transform,
target_link='link_5',
target_joint_bb=[[6, 1.2, 1.2]])
for j in range(15):
planner.ori_tol = 0.5 * j / 10.0
planner.pos_tol = 0.01 * j / 10.0
newplan = planner.plan()
if newplan.success():
plan = newplan
print '[Percept] hand planning success at the trial %d' % j
break
if plan:
plan.setSpeedByName('fastest')
plan.visualize()
if withPause:
print '[Percept] Going to percept_hand %s ' % caption[i]
pause()
plan.execute()
else:
print '[Percept] hand planning failed'
continue
# 3. percept
rospy.sleep(0.8) # wait for new pointcloud
obj_pose, score = capsen.capsen.detectOneObject(obj_id,
bin_contents,
binNum,
mode=1,
withScore=True)
if score > max_score:
max_obj_pose = obj_pose
max_score = score
# 4. move back to percept home
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize()
plan.setSpeedByName('yolo')
if withPause:
print '[Percept] Going back to percept_hand home'
pause()
plan.execute()
goToHome.goToHome(robotConfig=None,
homePos=[1, 0, 1.2],
isExecute=isExecute,
withPause=withPause)
if max_obj_pose is None:
return None, None, None
pose_type = find_object_pose_type(obj_id, max_obj_pose)
return max_obj_pose, pose_type, max_score