def topple(objPose = [1.55,0.25,1.1,0,0,0,0], binNum=0, objId = 0, bin_contents = None, robotConfig = None, shelfPosition = [1.9116,-0.012498,-0.4971], forceThreshold = 1, binDepthReach = 0.40, isExecute = True, withPause = True, withVisualize = True): ## objPose: world position and orientation frame attached to com of object ## 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 planSuccess = True ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) setSpeedByName(speedName = 'faster') joint_topic = '/joint_states' # shelf variables pretensionDelta = 0.00 lipHeight = 0.035 temp_bias = 0.00 # TO-DO remove this: # plan store plans = [] ## get object position (world frame) objPosition = getObjCOM(objPose[0:3], objId) ## move gripper to object com outside bin along world y direction and ## move the gripper over the lip of the bin # set tcp l1 = 0.018 l2 = 0.470 tip_hand_transform = [l1, 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] tcpPosHome = tcpPos toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist) closeGripper(forceThreshold) # if gripper open close it # set first target to move gripper in front of the object and adjust height to middle of bin distFromShelf = 0.05 wristWidth = 0.0725 # this is actually half the wrist width (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum) pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener) binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener) binFloorHeight = binFloorHeight.pose.position.z binMouth = [pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z] # set offset past COM # offset is 1/4 of object's height + half finger width # note: (COM height - bin floor height) is half of the object's height fingerWidth = 0.06 offsetFinger = fingerWidth/2 targetHeight = (objPosition[2] - binFloorHeight)*2 # to be changed to be from an object offsetEffect = targetHeight*3/4 offset = offsetEffect vertToppleHeight = binFloorHeight+offset-temp_bias ## bounding box constraint minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) # shelf frame # trying to topple an object that is too tall? maxHeight = coordinateFrameTransform([0,0,maxHeight], 'shelf', 'map', listener) #pdb.set_trace() targetPosition = [binMouth[0], objPosition[1], vertToppleHeight] binDepthReach = 0.3 binHeightSaftey = 0.01 if vertToppleHeight + binHeightSaftey > maxHeight.pose.position.z: fingerLength = 0.26 binDepth = 0.42 binDepthReach = binDepth - fingerLength + 0.08 toppleHeightWhenTall = binMouth[2] + 0.03 targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenTall] print '[Topple] Object too tall, topple will not go all the way in' # trying to topple an object that is too short? minHeight = coordinateFrameTransform([0,0,minHeight], 'shelf', 'map', listener) closeGripper(forceThreshold) if vertToppleHeight - binHeightSaftey < minHeight.pose.position.z: print '[Topple] Object too short, resetting topple height' toppleHeightWhenShort = minHeight.pose.position.z + binHeightSaftey targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenShort] gripper.move(80,100) # too far left or right? leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener) leftWall = leftWall.pose.position.y rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener) rightWall = rightWall.pose.position.y binLengthSafety = 0.02 horizontalSaftey = 0.01 if targetPosition[1] + binLengthSafety > leftWall: targetPosition[1] = leftWall - horizontalSaftey if targetPosition[1] - binLengthSafety < rightWall: targetPosition[1] = rightWall + horizontalSaftey frontOfObjectPtOutOfBin = targetPosition q_initial = robotConfig planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() effect = 'topple' if s and effect == 'topple': print '[Topple] move to COM in y and above COM in z successful' elif s and effect == 'slide': print '[Topple] move to COM in y and below COM in z successful' else: print '[Topple] move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility if s: visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Topple] move to COM in y and above COM in z fail' planSuccess = False qf = plan.q_traj[-1] ## perform bin length stroke to end q_initial = qf targetPosition = np.add(targetPosition, [binDepthReach,0,0]) planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() if s: print '[Topple] stroke to end of bin success' visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Topple] stroke to end of bin fail' planSuccess = False qf = plan.q_traj[-1] ## close gripper closeGripper(forceThreshold) ## retreat # go back along stroke q_initial = qf targetPosition = frontOfObjectPtOutOfBin planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() if s: print '[Topple] stroke to bin start success' visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Topple] stroke to bin start fail' planSuccess = False qf = plan.q_traj[-1] # go back to initial tcp position q_initial = qf targetPosition = frontOfObjectPtOutOfBin planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() if s: print '[Topple] return to mouth success' visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Topple] return to mouth fail' planSuccess = False if not planSuccess: return False, False ## execute #~ for numOfPlan in range(0, len(plans)): #~ plans[numOfPlan].visualize() #~ if isExecute: #~ pauseFunc(withPause) #~ plans[numOfPlan].execute() ## execute isNotInCollision = True for numOfPlan in range(0, len(plans)): plans[numOfPlan].visualize() if isExecute: pauseFunc(withPause) isNotInCollision = plans[numOfPlan].execute() if not isNotInCollision: print '[Topple] collision detected' planFailNum = numOfPlan break if not isNotInCollision: for numOfPlan in range(0, len(planFailNum)): plans[planFailNum-numOfPlan].executeBackward() ## retreat for numOfPlan in range(0, len(plans)): if withVisualize: plans[len(plans)-numOfPlan-1].visualizeBackward() if isExecute: pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() return True, False
def scoop(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1], binNum=3, objId=0, bin_contents=None, robotConfig=None, shelfPosition=[1.9116, -0.012498, -0.4971], forceThreshold=1, isExecute=True, withPause=True, withVisualize=False): ## 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 setSpeedByName(speedName='faster') joint_topic = '/joint_states' planSuccess = True ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) # shelf variables if binNum < 3: pretensionDelta = 0.03 if binNum > 2 and binNum < 6: pretensionDelta = 0.009 if binNum > 5 and binNum < 9: pretensionDelta = 0.009 if binNum > 8: pretensionDelta = 0.03 #pretensionDelta = 0.00 lipHeight = 0.025 # plan store plans = [] ## get object position (world frame) objPosition = getObjCOM(objPose[0:3], objId) ## move gripper to object com outside bin along world y direction and ## move the gripper over the lip of the bin # set tcp tcpXOffset = 0.018 l2 = 0.47 tip_hand_transform = [ tcpXOffset, 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 ] tcpPosHome = tcpPos # set scoop orientation (rotate wrist) scoopOrientation = [0, 0.7071, 0, 0.7071] # set first target to move gripper in front of the object and adjust height to middle of bin distFromShelf = 0.05 wristWidth = 0.0725 # this is actually half the wrist width (binMouth, binFloorHeight) = getBinMouthAndFloor(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 ] verticalOffsetLip = 0.00 # we need this so we don't damage the sucker wH = 0.075 targetPosition = [ binMouth[0], objPosition[1], binMouth[2] + tcpXOffset - wH + lipHeight - pretensionDelta ] ## check to make sure we are inside the bin and not colliding with sides: minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) leftWall = coordinateFrameTransform([leftWall, 0, 0], 'shelf', 'map', listener) leftWall = leftWall.pose.position.y rightWall = coordinateFrameTransform([rightWall, 0, 0], 'shelf', 'map', listener) rightWall = rightWall.pose.position.y interiorLipBin = [0, 0.40, 0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.20 sStroke = 0.19 binLengthSafety = 0.015 if targetPosition[1] + 0.04 > leftWall: interiorLipBin = [0, 0.36, 0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.17 if targetPosition[1] - 0.04 < leftWall: interiorLipBin = [0, 0.36, 0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.17 if targetPosition[1] + binLengthSafety > leftWall: targetPosition[1] = leftWall if targetPosition[1] - binLengthSafety < rightWall: targetPosition[1] = rightWall frontOfObjectPtOutOfBin = targetPosition q_initial = robotConfig planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 0 if s: print '[Scoop] move to COM in y successful' #~ print '[Scoop] tcp at:' #~ print(targetPosition) visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Scoop] move to COM in y fail' return (False, False) qf = plan.q_traj[-1] ## push spatula against base of bin (pre-tension) binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf', 'map', listener) q_initial = qf percentTilt = 0.1 # 10 percent scoopOrientation = [ 0, 0.7071 * (1 + percentTilt), 0, 0.7071 * (1 - percentTilt) ] planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 1 if s: print '[Scoop] reorient the hand' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] reorient the hand fail' return (False, False) qf = plan.q_traj[-1] # set second target, go over the lip of the bin deltaX = np.add(-targetPosition[0], stepOverLip) targetPosition = np.add(targetPosition, [deltaX, 0, 0]) q_initial = qf planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 2 if s: print '[Scoop] move to inside the lip success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] move to inside the lip fail' return (False, False) qf = plan.q_traj[-1] ## perform bin length stroke to middle q_initial = qf targetPosition = np.add(targetPosition, [fStroke, 0, -lipHeight]) planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 3 if s: print '[Scoop] stroke middle of bin success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] stroke middle of bin fail' return (False, False) qf = plan.q_traj[-1] ## perform bin length stroke to end q_initial = qf targetPosition = np.add(targetPosition, [sStroke, 0, 0]) scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4] planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 4 if s: print '[Scoop] stroke middle to end of bin success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] stroke middle to end of bin fail' return (False, False) qf = plan.q_traj[-1] ## close gripper #~ closeGripper(forceThreshold) closeGripper(forceThreshold) execution_possible = True ## execute isNotInCollision = True for numOfPlan in range(0, len(plans)): plans[numOfPlan].visualize() if isExecute: if numOfPlan == 3: openGripper() pauseFunc(withPause) isNotInCollision = plans[numOfPlan].execute() if numOfPlan == 3: closeGripper(forceThreshold) plans[numOfPlan].executeBackward() openGripper() plans[numOfPlan].execute() if not isNotInCollision: planFailNum = numOfPlan print '[Scoop] collision detected' break if numOfPlan == 4: closeGripper(forceThreshold) rospy.sleep(0.5) while True: APCrobotjoints = ROS_Wait_For_Msg( joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) == 2 or len(q0) == 8: q0 = q0[-2:] # get last 2 break gripper_q0 = np.fabs(q0) drop_thick = 0.000001 # finger gap =0.002m = .5 mm if gripper_q0[0] < drop_thick: print '[Scoop] ***************' print '[Scoop] Could not grasp' print '[Scoop] ***************' execution_possible = False else: print '[Scoop] ***************' print '[Scoop] Grasp Successful' print '[Scoop] ***************' execution_possible = True if not isNotInCollision: for numOfPlan in range(0, len(planFailNum)): plans[planFailNum - numOfPlan].executeBackward() ## retreat for numOfPlan in range(0, len(plans)): if withVisualize: plans[len(plans) - numOfPlan - 1].visualizeBackward() if isExecute: pauseFunc(withPause) plans[len(plans) - numOfPlan - 1].executeBackward() return True, execution_possible
def scoop(objPose = [1.95,0.25,1.4,0,0,0,1], binNum=3, objId = 0, bin_contents = None, robotConfig = None, shelfPosition = [1.9116,-0.012498,-0.4971], forceThreshold = 1, isExecute = True, withPause = True, withVisualize = False): ## 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 setSpeedByName(speedName = 'faster') joint_topic = '/joint_states' planSuccess = True ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) # shelf variables if binNum <3: pretensionDelta = 0.03 if binNum > 2 and binNum < 6: pretensionDelta = 0.009 if binNum > 5 and binNum < 9: pretensionDelta = 0.009 if binNum > 8: pretensionDelta = 0.03 #pretensionDelta = 0.00 lipHeight = 0.025 # plan store plans = [] ## get object position (world frame) objPosition = getObjCOM(objPose[0:3], objId) ## move gripper to object com outside bin along world y direction and ## move the gripper over the lip of the bin # set tcp tcpXOffset = 0.018 l2 = 0.47 tip_hand_transform = [tcpXOffset, 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] tcpPosHome = tcpPos # set scoop orientation (rotate wrist) scoopOrientation = [0, 0.7071, 0, 0.7071] # set first target to move gripper in front of the object and adjust height to middle of bin distFromShelf = 0.05 wristWidth = 0.0725 # this is actually half the wrist width (binMouth, binFloorHeight) = getBinMouthAndFloor(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] verticalOffsetLip = 0.00 # we need this so we don't damage the sucker wH = 0.075 targetPosition = [binMouth[0], objPosition[1], binMouth[2]+tcpXOffset-wH+lipHeight-pretensionDelta] ## check to make sure we are inside the bin and not colliding with sides: minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener) leftWall = leftWall.pose.position.y rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener) rightWall = rightWall.pose.position.y interiorLipBin = [0,0.40,0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.20 sStroke = 0.19 binLengthSafety = 0.015 if targetPosition[1] + 0.04 > leftWall: interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.17 if targetPosition[1] - 0.04 < leftWall: interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) stepOverLip = interiorLipBin.pose.position.x fStroke = 0.17 if targetPosition[1] + binLengthSafety > leftWall: targetPosition[1] = leftWall if targetPosition[1] - binLengthSafety < rightWall: targetPosition[1] = rightWall frontOfObjectPtOutOfBin = targetPosition q_initial = robotConfig planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 0 if s: print '[Scoop] move to COM in y successful' #~ print '[Scoop] tcp at:' #~ print(targetPosition) visualizeFunc(withVisualize, plan) plans.append(plan) else: print '[Scoop] move to COM in y fail' return (False, False) qf = plan.q_traj[-1] ## push spatula against base of bin (pre-tension) binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener) q_initial = qf percentTilt = 0.1 # 10 percent scoopOrientation = [0, 0.7071*(1+percentTilt), 0, 0.7071*(1-percentTilt)] planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 1 if s: print '[Scoop] reorient the hand' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] reorient the hand fail' return (False, False) qf = plan.q_traj[-1] # set second target, go over the lip of the bin deltaX = np.add(-targetPosition[0],stepOverLip) targetPosition = np.add(targetPosition, [deltaX,0,0]) q_initial = qf planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 2 if s: print '[Scoop] move to inside the lip success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] move to inside the lip fail' return (False, False) qf = plan.q_traj[-1] ## perform bin length stroke to middle q_initial = qf targetPosition = np.add(targetPosition, [fStroke,0,-lipHeight]) planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 3 if s: print '[Scoop] stroke middle of bin success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] stroke middle of bin fail' return (False, False) qf = plan.q_traj[-1] ## perform bin length stroke to end q_initial = qf targetPosition = np.add(targetPosition, [sStroke,0,0]) scoopOrientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4] planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() # Plan 4 if s: print '[Scoop] stroke middle to end of bin success' #~ print '[Scoop] tcp at:' #~ print(targetPosition) plans.append(plan) visualizeFunc(withVisualize, plan) else: print '[Scoop] stroke middle to end of bin fail' return (False, False) qf = plan.q_traj[-1] ## close gripper #~ closeGripper(forceThreshold) closeGripper(forceThreshold) execution_possible = True ## execute isNotInCollision = True for numOfPlan in range(0, len(plans)): plans[numOfPlan].visualize() if isExecute: if numOfPlan == 3: openGripper() pauseFunc(withPause) isNotInCollision = plans[numOfPlan].execute() if numOfPlan == 3: closeGripper(forceThreshold) plans[numOfPlan].executeBackward() openGripper() plans[numOfPlan].execute() if not isNotInCollision: planFailNum = numOfPlan print '[Scoop] collision detected' break if numOfPlan == 4: closeGripper(forceThreshold) rospy.sleep(0.5) while True: APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) == 2 or len(q0) == 8: q0 = q0[-2:] # get last 2 break gripper_q0=np.fabs(q0) drop_thick=0.000001 # finger gap =0.002m = .5 mm if gripper_q0[0] < drop_thick: print '[Scoop] ***************' print '[Scoop] Could not grasp' print '[Scoop] ***************' execution_possible = False else: print '[Scoop] ***************' print '[Scoop] Grasp Successful' print '[Scoop] ***************' execution_possible = True if not isNotInCollision: for numOfPlan in range(0, len(planFailNum)): plans[planFailNum-numOfPlan].executeBackward() ## retreat for numOfPlan in range(0, len(plans)): if withVisualize: plans[len(plans)-numOfPlan-1].visualizeBackward() if isExecute: pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() return True, execution_possible