def goToHome(robotConfig = None,
homePos = [1,0,1.2],
isExecute = True,
withPause = False):
## robotConfig: current time robot configuration
joint_topic = '/joint_states'
setSpeedByName(speedName = 'yolo')
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()
print ('[goToHome]')
#print home_joint_pose
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
return True
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 goToBin(binNum=0,
robotConfig=None,
objectiveBinPos=[1.2, 0, 0.6],
isExecute=True,
withPause=False,
withSuction=False,
counter=0):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
planSuctionSuccess = False
planGraspSuccess = False
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [
0, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# 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(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br,
pose=tip_hand_transform,
frame_id='target_pose',
parent_frame_id='map',
npub=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
# set scoop orientation (rotate wrist)
if not withSuction:
gripperOri = [0, 0.7071, 0, 0.7071]
if withSuction:
print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
gripperOri = [
pose_world.pose.orientation.x, pose_world.pose.orientation.y,
pose_world.pose.orientation.z, pose_world.pose.orientation.w
]
# move back 10 cms to avoid object with shelf collision
distAwayFromBin = 0.1
targetPosition = [tcpPos[0] - distAwayFromBin, tcpPos[1], tcpPos[2]]
q_initial = robotConfig
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()
s = plan.success()
if s:
print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
print '[goToBin] tcp at:', targetPosition
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] move away from bin fail'
return False
qf = plan.q_traj[-1]
if binNum > 8:
#plan = Plan()
#plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] # should use IKJoint
planner = IKJoint(
q0=qf,
target_joint_pos=[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745])
plan = planner.plan()
print '[goToBin] going home because risky to go straight to objective bin'
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
# move above objective bin, approx
use_JointPos = False
if use_JointPos:
planner = IKJoint(
q0=qf, target_joint_pos=[0, -0.2953, 0.4462, 0, 0.8292, 1.5707])
plan = planner.plan()
#plan = Plan()
#plan.q_traj = [[0, -0.2953, 0.4462, 0, 0.8292, 1.5707]] # should use IKJoint
s = True
else:
if not withSuction:
gripperOri = [0.7071, 0.7071, 0, 0]
locationAboveBinCOM = [
0, 0, 0.15
] # define over the lip distance in shelf frame
targetPosition = [
objectiveBinPos[0] + locationAboveBinCOM[0],
objectiveBinPos[1] + locationAboveBinCOM[1],
objectiveBinPos[2] + locationAboveBinCOM[2]
]
q_initial = qf
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()
s = plan.success()
if s:
print '[goToBin] move to above bin success'
planSuctionSuccess = True
plans.append(plan)
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
return False
if withSuction:
bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map',
listener)
targetPosition = [
bin_xyz.pose.position.x, bin_xyz.pose.position.y,
bin_xyz.pose.position.z
]
q_initial = qf
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()
s = plan.success()
print '[goToBin] setting joints for suction'
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
if q0[5] < 0:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834,
3.1415 - 2 * math.pi
]
else:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415
]
# plan_n = Plan()
# plan_n.q_traj=[possible_start_config]
planner = IKJoint(target_joint_pos=possible_start_config)
plan_n = planner.plan()
plan_n.visualize()
qf = plan_n.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan_n.execute()
## go down and release object
if not withSuction:
q_initial = qf
objectiveBinPos[1] = (counter - 1) * 0.036 - 0.2
targetPosition = [
objectiveBinPos[0], objectiveBinPos[1], objectiveBinPos[2]
]
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()
s = plan.success()
if s:
print '[goToBin] go to xyz of bin with vertical bias success'
plans.append(plan)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] go to xyz of bin with vertical bias fail'
qf = plan.q_traj[-1]
openGripper()
## open gripper fully
# rospy.sleep(0.5)
# openGripper()
rospy.sleep(0.5)
suction.stop()
return True
def suction_down(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
forceThreshold=1,
isExecute=True,
withPause=True):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
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]
vertical_index = np.argmax(np.fabs(obj_pose_orient[2, 0:3]))
object_dim = get_obj_dim(objId)
object_dim = adjust_obj_dim(objId, object_dim)
vertical_dim = object_dim[vertical_index]
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) >= 6:
q0 = q0[
0:
6] # take first 6, because in virtual environmet there will be additional 2 hand joint
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset = .035
l2 = .44
tip_hand_transform = [
-vert_offset, 0, l2, 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)
# 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
]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0.7071, 0, 0.7071, 0]
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, bin_height, bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
listener)
binMouth = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
hand_gap = 0
gripper.close()
finger_length = .23
finger_width = .08
up_offset = .05
down_offset = -.025
cup_to_spatula = .08
hand_width = .15
max_gripper_width = 110
hand_top_offset = hand_width / 2 + vert_offset + .01
hand_bot_offset = hand_width / 2 - vert_offset
#this determines bin stuff based on bin input
binFloorHeight = binMouth[2] - bin_height / 2
binCeilHeight = binMouth[2] + bin_height / 2
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall, binLarge) = getSides(binNum, listener)
horz_offset = 0
small_limit = binSmall + finger_width / 2 + horz_offset
large_limit = binLarge - finger_width / 2 - horz_offset
plans = []
plan = Plan()
if objPosition[1] < 0:
link6_angle = (q0[5] - math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518 - 2 * math.pi
]
else:
link6_angle = (q0[5] + math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518
]
#start_config=possible_start_config
start_config = [q0[0], q0[1], q0[2], q0[3], q0[4], link6_angle]
planner = IKJoint(q0=q0, target_joint_pos=start_config)
plan = planner.plan()
#plan.q_traj=[q0,start_config]
plans.append(plan)
qf = plan.q_traj[-1]
def get_motion_param(target_x, target_y):
object_depth = target_x - binMouth[0]
sidepos = min(max(target_y, small_limit), large_limit)
if object_depth < finger_length:
print '[SucDown] shallow suction'
h1 = binCeilHeight - lipHeight - cup_to_spatula
h2 = binFloorHeight + vertical_dim - down_offset
else:
print '[SucDown] deep suction, quitting'
h1 = binCeilHeight - lipHeight - hand_top_offset
h2a = binFloorHeight + vertical_dim - down_offset
h2b = binFloorHeight + hand_bot_offset + lipHeight
h2 = max(h2a, h2b)
return False, h1, h2, sidepos
h2 = max(h2, binFloorHeight)
if h2 > h1:
print '[SucDown] cant go in'
return False, h1, h2, sidepos
return True, h1, h2, sidepos
def generate_plan(targetPositionList, plans, qf):
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
planner = IK(q0=qf,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[SucDown] Plan number:', tp_index + 1
if s:
print '[SucDown] Plan calculated successfully'
plans.append(plan)
else:
print '[SucDown] Plan calulation failed'
return (False, plans, qf)
qf = plan.q_traj[-1]
return (True, plans, qf)
def execute_forward(plans, hand_gap):
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
def execute_backward(plans, hand_gap, plan_offset):
for numOfPlan in range(0, len(plans) - plan_offset):
plans[len(plans) - numOfPlan -
1].visualizeBackward(hand_param=hand_gap)
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
def try_suction(target_x, target_y, plans, qf, num_iter):
(continue_val, h1, h2, sidepos) = get_motion_param(target_x, target_y)
if continue_val == False:
return False, False
final_hand_gap = max_gripper_width
targetPositionList = [[target_x, sidepos, h1], [target_x, sidepos, h2]]
(continue_val, plans,
throwaway) = generate_plan(targetPositionList, plans, qf)
if continue_val == False:
return False, False
#set robot speed
setSpeedByName(speedName='faster')
hand_gap = 0
gripper.close()
execute_forward(plans, hand_gap)
#suction.start()
gripper.set_force(10)
gripper.grasp(move_pos=max_gripper_width)
gripper.close()
hand_gap = max_gripper_width
print '[SucDown] hand_gap:', hand_gap
execute_backward(plans, hand_gap, 0)
#time.sleep(4)
my_return = suction.check() or suction_override(objId)
if my_return == True:
#set robot speed
setSpeedByName(speedName='fast')
return (True, True)
else:
#suction.stop()
return (True, False)
target_offset = get_test_offset(objId)
target_x_list = [
objPosition[0], objPosition[0] - target_offset, objPosition[0],
objPosition[0], objPosition[0] + target_offset
]
target_y_list = [
objPosition[1], objPosition[1], objPosition[1] + target_offset,
objPosition[1] - target_offset, objPosition[1]
]
count = 0
suction_succeed = False
overall_plan_succeed = False
(continue_val, h1, h2,
sidepos) = get_motion_param(target_x=target_x_list[0],
target_y=target_y_list[0])
if continue_val == False:
return False, False
#targetPositionList=[
#[binMouth[0]-.15, sidepos, h1],
#[binMouth[0]-.15, sidepos, h1],
#[target_x_list[0], sidepos, h1]]
targetPositionListA = [[binMouth[0] - .15, sidepos, h1],
[binMouth[0] - .15, sidepos, h1]]
targetPositionListB = [[target_x_list[0], sidepos, h1]]
(continue_val, plans1, qf) = generate_plan(targetPositionListA, plans, qf)
if continue_val == False:
return False, False
(continue_val, plans2, qf) = generate_plan(targetPositionListB, [], qf)
if continue_val == False:
return False, False
setSpeedByName(speedName='yolo')
execute_forward(plans1, 0)
setSpeedByName(speedName='faster')
execute_forward(plans2, 0)
suction.start()
#time.sleep(6)
#plans_store=plans
plans = []
for count in range(0, len(target_x_list)):
(plan_succeed,
suction_succeed) = try_suction(target_x=target_x_list[count],
target_y=target_y_list[count],
plans=plans,
qf=qf,
num_iter=count)
if suction_succeed:
overall_plan_succeed = True
break
#return (plan_succeed,suction_succeed)
if plan_succeed == True:
overall_plan_succeed = True
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
qf = APCrobotjoints.position
if len(qf) < 6:
continue
else:
break
print '[SucDown] qf:', hand_gap
plans = []
count = count + 1
execute_backward(plans2, 0, 0)
if not suction_succeed:
suction.stop()
return (overall_plan_succeed, suction_succeed)
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
