def iterative_touch(self, touching_pose):
'''
Warning: this function now works for only left_arm
'''
step_size = self.topGraspingTouchTolerance / self.topGraspingTouchSteps
for i in range(1,self.topGraspingTouchSteps + 1):
if self._exit:
return False
touching_pose.p[2] += step_size
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, touching_pose, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
return True
except:
pass
return False
def touchRowBottom(torso_group, left_arm_group, row, ft = False):
base_frame_id = "/base_link"
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
touch_shelf_test_dict = rospy.get_param('/touch_shelf_test_dict')
break
except:
print 'here'
rospy.sleep(random.uniform(0,1))
continue
reach_in = touch_shelf_test_dict['reach_in']
pitch = touch_shelf_test_dict['pitch']
roll = touch_shelf_test_dict['roll']
preHeight = touch_shelf_test_dict['preHeight']
preDistance = touch_shelf_test_dict['preDistance']
trials = touch_shelf_test_dict['trials']
bottomHeight = touch_shelf_test_dict['row_height'][row]
if row == 'row_4':
bin = "shelf_bin_J"
elif row == 'row_3':
bin = "shelf_bin_G"
elif row == 'row_2':
bin = 'shelf_bin_D'
else:
rospy.logerr('row number not recognized')
return
while not rospy.is_shutdown():
try:
binFrame = listener.lookupTransform('/base_link', "/" + bin, rospy.Time(0))
break
except:
pass
try:
torso_group.set_joint_value_target(torso_joint_pos_dict['pregrasp'][row])
torso_group.go()
except:
rospy.logerr('can not move torso to detecting height')
return
tool_frame_rotation = kdl.Rotation.RPY(math.radians(roll), math.radians(pitch), 0)
'''
pregrasp
'''
init_touching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector(binFrame[0][0] + preDistance, binFrame[0][1] + 0.14, binFrame[0][2] + preHeight))
try:
pr2_moveit_utils.go_tool_frame(left_arm_group, init_touching_pose, base_frame_id = base_frame_id, ft = ft,
wait=True, tool_x_offset = tool_size[0])
except:
rospy.logerr('failed to init top grasping pose')
return
'''
reaching
'''
reaching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector(binFrame[0][0] + reach_in, binFrame[0][1] + 0.14, binFrame[0][2] + preHeight))
try:
pr2_moveit_utils.go_tool_frame(left_arm_group, reaching_pose, base_frame_id = base_frame_id, ft = ft,
wait=True, tool_x_offset = tool_size[0])
except:
rospy.logerr('failed to reach top grasping pose')
return
'''
touching
'''
stepDistance = (binFrame[0][2] + preHeight - bottomHeight) / (trials - 1)
for i in range(trials):
height = binFrame[0][2] + preHeight - i * stepDistance
touching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector(binFrame[0][0] + reach_in, binFrame[0][1] + 0.14, height))
try:
pr2_moveit_utils.go_tool_frame(left_arm_group, touching_pose, base_frame_id = base_frame_id, ft = ft,
wait=True, tool_x_offset = tool_size[0])
except:
rospy.logerr('best touch height found is (heigher than expected): %4f' % height)
return
rospy.logerr('best touch height found is: %4f' % height)
return
def sideGrasping(self):
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
if self._exit:
return False
try:
tp = self.listener.lookupTransform('/base_link', "/" + self._item + "_detector", rospy.Time(0))
binFrame = self.listener.lookupTransform("/" + "shelf_" + self._bin, "/" + self._item + "_detector", rospy.Time(0))
liftShift = 0.15 - binFrame[0][1]
rospy.loginfo('got new object pose')
tpRPY = self.RPYFromQuaternion(tp[1])
objBinRPY = self.RPYFromQuaternion(binFrame[1])
planner_frame = '/' + self._item + "_detector"
self.poseFromSimtrack = True
break
except:
try:
tp = self.listener.lookupTransform('/base_link', "/" + self._item + "_detector_seg", rospy.Time(0))
binFrame = self.listener.lookupTransform("/" + "shelf_" + self._bin, "/" + self._item + "_detector_seg", rospy.Time(0))
planner_frame = '/' + self._item + "_detector_seg"
liftShift = 0.15 - binFrame[0][1]
rospy.loginfo('got new object pose')
tpRPY = self.RPYFromQuaternion(tp[1])
objBinRPY = self.RPYFromQuaternion(binFrame[1])
self.poseFromSimtrack = False
break
except:
r.sleep()
continue
self.open_left_gripper()
row_height = self.grasping_param_dict['row_height'][self.get_row()]
if self._item == 'laugh_out_loud_joke_book' or self._item == 'mark_twain_huckleberry_finn' or self._item == 'stanley_66_052' or self._item == 'sharpie_accent_tank_style_highlighters':
if binFrame[0][1] > 0.14:
bookX = self.sideGraspingBookX
bookY = self.sideGraspingBookY
bookZ = self.sideGraspingBookZ
else:
bookX = self.sideGraspingBookX
bookY = -self.sideGraspingBookY
bookZ = self.sideGraspingBookZ
else:
bookX = 0
bookY = 0
bookZ = 0
if self._item == 'sharpie_accent_tank_style_highlighters':
bookz = 0.03
if tp[0][2] - row_height < 0.03:
rospy.logerr('object center too low for sideGrasping')
return False
if abs(objBinRPY[1]) > 0.5:
rospy.logerr('require pushing the object')
return False
angle_step = 0
if objBinRPY[2] < 0:
angle_step = -self.sideGraspingTolerance / (self.sideGraspingTrialAngles - 1.0)
else:
angle_step = self.sideGraspingTolerance / (self.sideGraspingTrialAngles - 1.0)
for i in range(self.sideGraspingTrialAngles):
if self._exit:
return False
yaw_now = math.radians(angle_step * i)
x_shift_now = (self.pre_distance + self.xLength) * math.cos(yaw_now)
y_shift_now = (self.pre_distance + self.xLength) * math.sin(yaw_now)
rospy.loginfo('yaw_now: %4f, y_shift_now: %4f' % (yaw_now, y_shift_now))
'''
PRE-GRASPING
'''
rospy.loginfo('PRE-GRASPING')
rospy.logerr(yaw_now)
if self.get_row() == 'row_1':
rospy.loginfo('row_1 grasping')
pre_pose = kdl.Frame(kdl.Rotation.RPY(0, math.radians(self.sideGraspingRow1_pitch), yaw_now), kdl.Vector( x_shift_now, y_shift_now + bookY, bookZ))
else:
pre_pose = kdl.Frame(kdl.Rotation.RPY(0, 0, yaw_now), kdl.Vector( x_shift_now, y_shift_now + bookY, bookZ))
pre_pose_robot = self.transformPoseToRobotFrame(pre_pose, planner_frame)
if self._exit:
return False
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, pre_pose_robot.pose, base_frame_id = pre_pose_robot.header.frame_id, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except Exception, e:
rospy.logerr('exception in PRE-GRASPING')
continue
'''
REACHING
'''
# row_1_reach =
rospy.loginfo('REACHING')
if self.poseFromSimtrack:
if self.get_row == 'row_1':
reaching_pose = kdl.Frame(kdl.Rotation.RPY(math.radians(self.sideGraspingRow1_pitch), 0, yaw_now), kdl.Vector( bookX, bookY, bookZ))
else:
reaching_pose = kdl.Frame(kdl.Rotation.RPY(0, 0, yaw_now), kdl.Vector( bookX, bookY, bookZ))
reaching_pose_robot = self.transformPoseToRobotFrame(reaching_pose, planner_frame)
else:
if self.get_row() == 'row_1':
reaching_pose = kdl.Frame(kdl.Rotation.RPY(math.radians(self.sideGraspingRow1_pitch), 0, yaw_now), kdl.Vector( self.sideGraspingSegReach + bookX, bookY, bookZ))
else:
reaching_pose = kdl.Frame(kdl.Rotation.RPY(0, 0, yaw_now), kdl.Vector( self.sideGraspingSegReach + bookX, bookY, 0))
reaching_pose_robot = self.transformPoseToRobotFrame(reaching_pose, planner_frame)
if self._exit:
return False
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, reaching_pose_robot.pose, base_frame_id = reaching_pose_robot.header.frame_id, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
rospy.logerr('exception in REACHING')
continue
'''
GRASPING
'''
rospy.loginfo('GRASPING')
self.close_left_gripper()
'''
CHECKING THE GRIPPER ANGLE POS
'''
rospy.loginfo('[grasp_object]: Checking gripper angle pos')
# action has failed... return false
if not self.check_gripper_angle():
rospy.logerr('[grasp_object]: grasping action failed')
self.open_left_gripper()
continue
'''
LIFTING
'''
rospy.loginfo('LIFTING')
if self.get_row() == 'row_1':
curr_joints = self.left_arm.get_current_joint_values()
curr_joints[5] = self.sideGraspingRow1_liftAngle
self.left_arm.set_joint_value_target(curr_joints)
try:
self.left_arm.go()
except:
self.open_left_gripper()
rospy.logerr('exception in LIFTING')
continue
else:
lifting_pose = kdl.Frame(kdl.Rotation.RPY(tpRPY[0], tpRPY[1], 0), kdl.Vector( tp[0][0], tp[0][1] + liftShift, tp[0][2] + self.lifting_height))
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, lifting_pose, base_frame_id = 'base_link', ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
self.open_left_gripper()
rospy.logerr('exception in LIFTING')
continue
'''
RETREATING
'''
rospy.loginfo('RETREATING')
try:
column = self.get_column()
base_pos_goal = self.base_pos_dict[column]
new_base_pose = copy.deepcopy(base_pos_goal)
new_base_pose[0] -= abs(self.base_retreat_distance)
self.go_base_pos_async(new_base_pose)
except Exception, e:
# rospy.logerr(e)
self.flush()
self.open_left_gripper()
rospy.logerr('exception in RETREATING')
continue
def topGrasping(self):
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
if self._exit:
return False
try:
tp = self.listener.lookupTransform('/base_link', "/" + self._item + "_detector", rospy.Time(0))
binFrame = self.listener.lookupTransform("/" + "shelf_" + self._bin, "/" + self._item + "_detector", rospy.Time(0))
liftShift = 0.15 - binFrame[0][1]
rospy.loginfo('got new object pose')
tpRPY = self.RPYFromQuaternion(tp[1])
self.poseFromSimtrack = True
break
except:
try:
tp = self.listener.lookupTransform('/base_link', "/" + self._item + "_detector_seg", rospy.Time(0))
binFrame = self.listener.lookupTransform("/" + "shelf_" + self._bin, "/" + self._item + "_detector_seg", rospy.Time(0))
liftShift = 0.13 - binFrame[0][1]
rospy.loginfo('got new object pose')
tpRPY = self.RPYFromQuaternion(tp[1])
self.poseFromSimtrack = False
break
except:
r.sleep()
continue
self.open_left_gripper()
if self._exit:
return False
for i in range(self.topGraspingPitchTrials):
if self._exit:
return False
tgp = self.topGraspingPitch - self.topGraspingPitchTolerance / (self.topGraspingPitchTrials - 1) * i
rospy.loginfo('topGraspingPitch now: %4f' % tgp)
for j in range(self.topGraspingYawTrials):
if self._exit:
return False
y_shift_step = 0.06 / (self.topGraspingYawTrials - 1.0)
reach_Y_shift_step = self.topGraspingYshiftTolerance / (self.topGraspingYawTrials - 1.0)
if binFrame[0][1] >= 0.13:
tgy = self.topGraspingYawTolerance / (self.topGraspingYawTrials - 1) * j
y_shift_now = - y_shift_step * j
reach_Y_shift = - reach_Y_shift_step * j
else:
tgy = -self.topGraspingYawTolerance / (self.topGraspingYawTrials - 1) * j
y_shift_now = y_shift_step * j
reach_Y_shift = reach_Y_shift_step * j
rospy.loginfo('topGraspingYaw now: %4f' % tgy)
row_height = self.grasping_param_dict['row_height'][self.get_row()]
tool_frame_rotation = kdl.Rotation.RPY(math.radians(self.topGraspingRoll), math.radians(tgp), math.radians(tgy))
reachingHeight = min(self.topGraspingMaxReachingHeight + row_height, tp[0][2] + self.topGraspHeight)
'''
PRE-GRASPING
'''
pre_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0] + self.topGrasping_pre_distance, tp[0][1] + y_shift_now, reachingHeight))
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, pre_pose, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
rospy.logerr('exception in PRE-GRASPING')
continue
'''
REACHING
'''
if self.poseFromSimtrack:
reaching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] + reach_Y_shift, tp[0][2] + self.topGraspHeight))
else:
reaching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0] + self.topGraspingReachSeg, tp[0][1] + reach_Y_shift, reachingHeight))
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, reaching_pose, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
rospy.logerr('exception in REACHING')
continue
'''
TOUCHING
'''
touching_height = max(tp[0][2], row_height)
if self.poseFromSimtrack:
touching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] + reach_Y_shift, touching_height))
else:
touching_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0] + self.topGraspingReachSeg, tp[0][1] + reach_Y_shift, touching_height))
rospy.loginfo("touching_height: %4f" % touching_height)
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, touching_pose, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
if not self.iterative_touch(touching_pose):
rospy.logerr('exception in TOUCHING')
continue
else:
pass
'''
SMART-SHAKING
'''
if self.poseFromSimtrack:
shaking_pose1 = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] + 0.01, touching_height))
shaking_pose2 = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] - 0.01, touching_height))
else:
shaking_pose1 = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0] + self.topGraspingReachSeg, tp[0][1] + reach_Y_shift + 0.02, touching_height))
shaking_pose2 = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0] + self.topGraspingReachSeg, tp[0][1] + reach_Y_shift - 0.02, touching_height))
for i in range(self.topGraspingShakingNumber):
if self._exit:
return False
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, shaking_pose1, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
rospy.logerr('exception in SMART-SHAKING left, never mind')
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, shaking_pose2, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
rospy.logerr('exception in SMART-SHAKING right, never mind')
'''
GRASPING
'''
self.close_left_gripper()
'''
LIFTING
'''
if self._item == 'dr_browns_bottle_brush':
lifting_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] + liftShift, tp[0][2] + self.topGraspingLiftingHeight + 0.04))
else:
lifting_pose = kdl.Frame(tool_frame_rotation, kdl.Vector( tp[0][0], tp[0][1] + liftShift, tp[0][2] + self.topGraspingLiftingHeight))
try:
pr2_moveit_utils.go_tool_frame(self.left_arm, lifting_pose, base_frame_id = self.topGraspingFrame, ft=self.ft_switch,
wait=True, tool_x_offset=self._tool_size[0])
except:
self.open_left_gripper()
rospy.logerr('exception in LIFTING')
continue
'''
RETREATING
'''
rospy.loginfo('RETREATING')
try:
column = self.get_column()
base_pos_goal = self.base_pos_dict[column]
new_base_pose = copy.deepcopy(base_pos_goal)
new_base_pose[0] -= abs(self.base_retreat_distance)
self.go_base_pos_async(new_base_pose)
except Exception, e:
self.open_left_gripper()
rospy.logerr('exception in RETREATING')
continue
return True
while not rospy.is_shutdown():
try:
T_b_groovy_tf = listener.lookupTransform('/base_link', '/amazon_cheeze', rospy.Time(0))
print "got new pose"
except:
continue
curr_pose = left_arm.get_current_pose()
target_pose = kdl.Frame(kdl.Rotation.RPY(0,0,0), tf2msg(T_b_groovy_tf,curr_pose))
try:
print 'going to tracked position'
pr2_moveit_utils.go_tool_frame(left_arm, target_pose, base_frame_id ='base_link', ft=False, wait=True)
print 'position reached'
except:
print "goal position not reachable"
print ""
print ''
continue
# left_arm.set_pose_target(tf2msg(T_b_groovy_tf,curr_pose))
# left_arm.go()
# r.sleep()
T_b_groovy_tf = listener.lookupTransform('/base_link',
'/amazon_cheeze',
rospy.Time(0))
print "got new pose"
except:
continue
curr_pose = left_arm.get_current_pose()
target_pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0),
tf2msg(T_b_groovy_tf, curr_pose))
try:
print 'going to tracked position'
pr2_moveit_utils.go_tool_frame(left_arm,
target_pose,
base_frame_id='base_link',
ft=False,
wait=True)
print 'position reached'
except:
print "goal position not reachable"
print ""
print ''
continue
# left_arm.set_pose_target(tf2msg(T_b_groovy_tf,curr_pose))
# left_arm.go()
# r.sleep()
