def goToMouth(robotConfig = None, binNum = 0, isExecute = True, withPause = False): ## robotConfig: current time robot configuration 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 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 home orientation gripperOri = [0, 0.7071, 0, 0.7071] # move to bin mouth distFromShelf = 0.15 mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z] 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 '[goToMouth] move to bin mouth successful' plan.visualize() plans.append(plan) if isExecute: pauseFunc(withPause) plan.execute() else: print '[goToMouth] move to bin mouth fail' return None qf = plan.q_traj[-1] ## open gripper fully openGripper() return plan
def scoop(objPose = [1.95,0.25,1.4], objId = 0, binNum=3, robotConfig=[0,0,0,0,0,0], shelfPosition = [1.91,0,-0.50], forceThreshold = 1, isExecute = False): ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix ## objId: object identifier ## robotConfig: current time robot configuration. List of joint angles, in radians ## shelfPosition: shelf position in world frame. This is a list. ## force threshold: amount of force needed to say we have a grasp ## initialize listener rospy ## Copy pasta hasta la pasta rospy.init_node('listener', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) ##hasta la copy la patio ## stop pasta # shelf variables pretensionDelta = 0.00 #how far we push nail down lipHeight = 0.035 #height of the lip of the bin # plan store plans = [] #list of plans generated each plan is an object ## get object position (world frame) objPosition = getObjCOM(objPose, objId) # ## move gripper to object com outside bin along world y direction and ## move the gripper over the lip of the bin # set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6 l2 = 0.45 #how far tcp is away from link 6 tip_hand_transform = [0, 0, l2, 0,0,0] #something used for planning in drake, I don't get it # to be updated when we have a hand design finalized # broadcast frame attached to tcp rospy.sleep(0.1) #.1 second delay so broadcasting works. syncing with ROS #broadcasting transformation between tcp and link 6 on ROS 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) # get position of the tcp in world frame pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) #tcp position as a list 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] targetPosition = [binMouth[0], objPosition[1], binMouth[2]] #align the position of the tcp relative to the object before the scoop frontOfObjectPtOutOfBin = targetPosition q_initial = robotConfig #q0 is initial configuration #target_tip_pos is where we want the tcp to be #target_tip_ori is the tip orientation #tip hand transform is see above planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() #true if successful if s: print 'move to COM in y successful' print 'tcp at:' print(targetPosition) plan.visualize() #display in arvix plans.append(plan) if isExecute: plan.execute() #this executes the plan for reals else: print 'move to COM in y fail' #after this point, it's just a sequence of points for the plan qf = plan.q_traj[-1] #this is the final configuration of the object # set second target, go over the lip of the bin interiorLipBin = [0,0.37,0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) deltaX = np.add(-targetPosition[0],interiorLipBin.pose.position.x) 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) plan = planner.plan() s = plan.success() if s: print 'move to inside the lip success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'move to inside the lip fail' qf = plan.q_traj[-1] ## open gripper fully openGripper() ## push spatula against base of bin (pre-tension) binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener) q_initial = qf deltaH = targetPosition[2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight targetPosition = np.add(targetPosition, [0,0,-deltaH+wristWidth]) scoopOrientation = [0, 0.7071+0.11/2, 0, 0.7071-0.11/2] planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() if s: print 'push finger against floor of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'push finger against floor of bin fail' qf = plan.q_traj[-1] ## perform bin length stroke to middle q_initial = qf targetPosition = np.add(targetPosition, [0.20,0,-lipHeight]) planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() if s: print 'stroke middle of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'stroke middle of bin fail' qf = plan.q_traj[-1] plans.append(plan) ## perform bin length stroke to end q_initial = qf targetPosition = np.add(targetPosition, [0.20,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) plan = planner.plan() s = plan.success() if s: print 'stroke middle to end of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'stroke middle to end of bin fail' qf = plan.q_traj[-1] plans.append(plan) ## close gripper closeGripper(forceThreshold) ## retreat #pdb.set_trace() #TRACEEEEE #this does each plan backwards so that the hand can leave the bin #VERY IMPORTANT for numOfPlan in range(0, len(plans)): plans[len(plans)-numOfPlan-1].visualizeBackward() if isExecute: for numOfPlan in range(0, len(plans)): plans[len(plans)-numOfPlan-1].executeBackward()
def scoop(objPose=[1.95, 0.25, 1.4], objId=0, binNum=3, robotConfig=[0, 0, 0, 0, 0, 0], shelfPosition=[1.91, 0, -0.50], forceThreshold=1, isExecute=False): ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix ## objId: object identifier ## robotConfig: current time robot configuration. List of joint angles, in radians ## shelfPosition: shelf position in world frame. This is a list. ## force threshold: amount of force needed to say we have a grasp ## initialize listener rospy ## Copy pasta hasta la pasta rospy.init_node('listener', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) ##hasta la copy la patio ## stop pasta # shelf variables pretensionDelta = 0.00 #how far we push nail down lipHeight = 0.035 #height of the lip of the bin # plan store plans = [] #list of plans generated each plan is an object ## get object position (world frame) objPosition = getObjCOM(objPose, objId) # ## move gripper to object com outside bin along world y direction and ## move the gripper over the lip of the bin # set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6 l2 = 0.45 #how far tcp is away from link 6 tip_hand_transform = [ 0, 0, l2, 0, 0, 0 ] #something used for planning in drake, I don't get it # to be updated when we have a hand design finalized # broadcast frame attached to tcp rospy.sleep(0.1) #.1 second delay so broadcasting works. syncing with ROS #broadcasting transformation between tcp and link 6 on ROS 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) # get position of the tcp in world frame pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) #tcp position as a list 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 ] targetPosition = [ binMouth[0], objPosition[1], binMouth[2] ] #align the position of the tcp relative to the object before the scoop frontOfObjectPtOutOfBin = targetPosition q_initial = robotConfig #q0 is initial configuration #target_tip_pos is where we want the tcp to be #target_tip_ori is the tip orientation #tip hand transform is see above planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() #true if successful if s: print 'move to COM in y successful' print 'tcp at:' print(targetPosition) plan.visualize() #display in arvix plans.append(plan) if isExecute: plan.execute() #this executes the plan for reals else: print 'move to COM in y fail' #after this point, it's just a sequence of points for the plan qf = plan.q_traj[-1] #this is the final configuration of the object # set second target, go over the lip of the bin interiorLipBin = [0, 0.37, 0] # define over the lip distance in shelf frame interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener) deltaX = np.add(-targetPosition[0], interiorLipBin.pose.position.x) 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) plan = planner.plan() s = plan.success() if s: print 'move to inside the lip success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'move to inside the lip fail' qf = plan.q_traj[-1] ## open gripper fully openGripper() ## push spatula against base of bin (pre-tension) binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf', 'map', listener) q_initial = qf deltaH = targetPosition[ 2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight targetPosition = np.add(targetPosition, [0, 0, -deltaH + wristWidth]) scoopOrientation = [0, 0.7071 + 0.11 / 2, 0, 0.7071 - 0.11 / 2] planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() if s: print 'push finger against floor of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'push finger against floor of bin fail' qf = plan.q_traj[-1] ## perform bin length stroke to middle q_initial = qf targetPosition = np.add(targetPosition, [0.20, 0, -lipHeight]) planner = IK(q0=q_initial, target_tip_pos=targetPosition, target_tip_ori=scoopOrientation, tip_hand_transform=tip_hand_transform) plan = planner.plan() s = plan.success() if s: print 'stroke middle of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'stroke middle of bin fail' qf = plan.q_traj[-1] plans.append(plan) ## perform bin length stroke to end q_initial = qf targetPosition = np.add(targetPosition, [0.20, 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) plan = planner.plan() s = plan.success() if s: print 'stroke middle to end of bin success' print 'tcp at:' print(targetPosition) plans.append(plan) plan.visualize() if isExecute: plan.execute() else: print 'stroke middle to end of bin fail' qf = plan.q_traj[-1] plans.append(plan) ## close gripper closeGripper(forceThreshold) ## retreat #pdb.set_trace() #TRACEEEEE #this does each plan backwards so that the hand can leave the bin #VERY IMPORTANT for numOfPlan in range(0, len(plans)): plans[len(plans) - numOfPlan - 1].visualizeBackward() if isExecute: for numOfPlan in range(0, len(plans)): plans[len(plans) - numOfPlan - 1].executeBackward()
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 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 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 goToMouth(robotConfig=None, binNum=0, isExecute=True, withPause=False): ## robotConfig: current time robot configuration 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 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 home orientation gripperOri = [0, 0.7071, 0, 0.7071] # move to bin mouth distFromShelf = 0.15 mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [ mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z ] 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 '[goToMouth] move to bin mouth successful' plan.visualize() plans.append(plan) if isExecute: pauseFunc(withPause) plan.execute() else: print '[goToMouth] move to bin mouth fail' return None qf = plan.q_traj[-1] ## open gripper fully openGripper() return plan
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