def getSides(objX,listener): bin_cnstr = get_bin_cnstr() #X_list=[-0.42926,-0.1494,0.1554,0.42926] X_list=[bin_cnstr[2][0],bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]] print X_list #AdjustedX_list=[-0.42926+.045,-0.1494,0.1554,0.42926-.045] #adjust for the lips on far right/left sides AdjustedX_list=[bin_cnstr[2][0]+.045,bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]-.045] print AdjustedX_list objXVec = [0,objX,0] newObjXVec = coordinateFrameTransform(objXVec, 'map', 'shelf', listener) newObjX = newObjXVec.pose.position.x for i in range(0, len(X_list)-1): if (X_list[i]<=newObjX) and (newObjX<=X_list[i+1]): shelf_gap=AdjustedX_list[i+1]-AdjustedX_list[i] shelfXVec=[AdjustedX_list[i+1],0,0] newShelfXVec=coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener) return (newShelfXVec.pose.position.y-shelf_gap,newShelfXVec.pose.position.y) return 'failed'
def getCeilHeight(objHeight, listener): bin_cnstr = get_bin_cnstr() #height_list=[0.800027,1.06673,1.29533,1.52393,1.79063] height_list = [ bin_cnstr[11][4], bin_cnstr[8][5], bin_cnstr[5][5], bin_cnstr[2][5], bin_cnstr[11][6] ] print '[SucSide] height_list:', height_list objHeightVec = [0, 0, objHeight] # define over the lip distance in shelf frame newObjHeightVec = coordinateFrameTransform(objHeightVec, 'map', 'shelf', listener) newObjHeight = newObjHeightVec.pose.position.z print '[SucSide] objHeight:', objHeight for i in range(0, len(height_list) - 1): if (height_list[i] <= newObjHeight) and (newObjHeight <= height_list[i + 1]): shelf_gap = height_list[i + 1] - height_list[i] shelfHeightVec = [0, 0, height_list[i + 1]] newShelfHeightVec = coordinateFrameTransform( shelfHeightVec, 'shelf', 'map', listener) return (newShelfHeightVec.pose.position.z - shelf_gap, newShelfHeightVec.pose.position.z) return '[SucSide] getCeilHeight failed'
def recover(obj_frame_id, global_frame_id, z, slot_pos_obj, reset): global globalvel global global_slow_vel zup = z + 0.03 ori = [0, 0, 1, 0] center_world = [0.35, 0, 0] slot_pos_obj = slot_pos_obj + [0] if reset: # move above the slot pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = zup setCart(pos_recover_probe_world, ori) # speed down setSpeed(tcp=global_slow_vel, ori=1000) # move down to the slot pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = z setCart(pos_recover_probe_world, ori) #pause() # move to the world center pos_recover_probe_target_world = center_world pos_recover_probe_target_world[2] = z setCart(pos_recover_probe_target_world, ori) # speed up setSpeed(tcp=globalvel, ori=1000) # move to the world center pos_recover_probe_target_world = center_world pos_recover_probe_target_world[2] = zup+0.03 # up more to let vicon see the marker setCart(pos_recover_probe_target_world, ori) setCart([0.2, 0, z + 0.05], ori) # special
def recover(slot_pos_obj, reset): global z, zup #import pdb; pdb.set_trace() z_recover = 0.016 + z # the height for recovery z_ofset = z_recover slot_pos_obj = slot_pos_obj + [0] _center_world = copy.deepcopy(center_world) if reset: # move above the slot pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = zup setCart(pos_recover_probe_world, ori) # speed down setSpeed(tcp=global_slow_vel, ori=1000) # move down to the slot pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = z_ofset setCart(pos_recover_probe_world, ori) #pause() # move to the world center pos_recover_probe_target_world = _center_world pos_recover_probe_target_world[2] = z_ofset setCart(pos_recover_probe_target_world, ori) # speed up setSpeed(tcp=globalvel, ori=1000) # move up pos_recover_probe_target_world = _center_world pos_recover_probe_target_world[2] = zup + 0.06 # setCart(pos_recover_probe_target_world, ori)
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 recover(slot_pos_obj, reset): global z, zup #import pdb; pdb.set_trace() z_recover = 0.016 + z # the height for recovery z_ofset = z_recover slot_pos_obj = slot_pos_obj + [0] _center_world = copy.deepcopy(center_world) if reset: # move above the slot pos_recover_probe_world = coordinateFrameTransform( slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = zup setCart(pos_recover_probe_world, ori) # move down a bit in fast speed pos_recover_probe_world = coordinateFrameTransform( slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = z_ofset + 0.02 setCart(pos_recover_probe_world, ori) # move down to the slot # speed down setAcc(acc=globalhighacc, deacc=globalacc) setSpeed(tcp=60, ori=1000) pos_recover_probe_world[2] = z_ofset setCart(pos_recover_probe_world, ori) #pause() # move to the world center pos_recover_probe_target_world = _center_world pos_recover_probe_target_world[2] = z_ofset setCart(pos_recover_probe_target_world, ori) # speed up setAcc(acc=globalhighacc, deacc=globalhighacc) setSpeed(tcp=global_highvel, ori=1000) # move up pos_recover_probe_target_world = _center_world pos_recover_probe_target_world[2] = zup + 0.06 # setCart(pos_recover_probe_target_world, ori)
def getCeilHeight(objHeight,listener): bin_cnstr = get_bin_cnstr() #height_list=[0.800027,1.06673,1.29533,1.52393,1.79063] height_list=[bin_cnstr[11][4],bin_cnstr[8][5],bin_cnstr[5][5],bin_cnstr[2][5],bin_cnstr[11][6]] print height_list objHeightVec = [0,0,objHeight] # define over the lip distance in shelf frame newObjHeightVec = coordinateFrameTransform(objHeightVec, 'map', 'shelf', listener) newObjHeight = newObjHeightVec.pose.position.z print objHeight for i in range(0, len(height_list)-1): if (height_list[i]<=newObjHeight) and (newObjHeight<=height_list[i+1]): shelf_gap=height_list[i+1]-height_list[i] shelfHeightVec=[0,0,height_list[i+1]] newShelfHeightVec=coordinateFrameTransform(shelfHeightVec, 'shelf', 'map', listener) return (newShelfHeightVec.pose.position.z-shelf_gap,newShelfHeightVec.pose.position.z) return 'failed'
def recover(obj_frame_id, global_frame_id, z, slot_pos_obj, reset): global globalvel global global_slow_vel zup = z + 0.03 ori = [0, 0, 1, 0] center_world = [0.35, 0, 0] slot_pos_obj = slot_pos_obj + [0] if reset: # move above the slot pos_recover_probe_world = coordinateFrameTransform( slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = zup setCart(pos_recover_probe_world, ori) # speed down setSpeed(tcp=global_slow_vel, ori=1000) # move down to the slot pos_recover_probe_world = coordinateFrameTransform( slot_pos_obj, obj_frame_id, global_frame_id, listener) pos_recover_probe_world[2] = z setCart(pos_recover_probe_world, ori) #pause() # move to the world center pos_recover_probe_target_world = center_world pos_recover_probe_target_world[2] = z setCart(pos_recover_probe_target_world, ori) # speed up setSpeed(tcp=globalvel, ori=1000) # move to the world center pos_recover_probe_target_world = center_world pos_recover_probe_target_world[ 2] = zup + 0.03 # up more to let vicon see the marker setCart(pos_recover_probe_target_world, ori) setCart([0.2, 0, z + 0.05], ori) # special
def getSides(binNum,listener): bin_cnstr = get_bin_cnstr() AdjustedX_list=[bin_cnstr[2][0]+.045,bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]-.045] print 'AdjustedX_list:', AdjustedX_list i=2-(binNum%3) shelf_gap=AdjustedX_list[i+1]-AdjustedX_list[i] shelfXVec=[AdjustedX_list[i+1],0,0] newShelfXVec=coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener) return (newShelfXVec.pose.position.y-shelf_gap,newShelfXVec.pose.position.y)
def getSides(binNum, listener): bin_cnstr = get_bin_cnstr() AdjustedX_list = [ bin_cnstr[2][0] + .045, bin_cnstr[2][1], bin_cnstr[1][1], bin_cnstr[0][1] - .045 ] print '[SucDown] AdjustedX_list:', AdjustedX_list i = 2 - (binNum % 3) shelf_gap = AdjustedX_list[i + 1] - AdjustedX_list[i] shelfXVec = [AdjustedX_list[i + 1], 0, 0] newShelfXVec = coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener) return (newShelfXVec.pose.position.y - shelf_gap, newShelfXVec.pose.position.y)
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 = 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 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: continue else: 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] 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) 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]; plan.q_traj=[q0,start_config] plans.append(plan) qf=plan.q_traj[-1] def try_suction(target_x,target_y,qf,num_iter): object_depth=target_x-binMouth[0] sidepos=min(max(target_y,small_limit),large_limit) if object_depth<finger_length: print 'shallow suction' h1=binCeilHeight-lipHeight-cup_to_spatula h2=binFloorHeight+vertical_dim-down_offset else: print 'deep suction' h1=binCeilHeight-lipHeight-hand_top_offset h2a=binFloorHeight+vertical_dim-down_offset h2b=binFloorHeight+hand_bot_offset+lipHeight h2=max(h2a,h2b) h2=max(h2,binFloorHeight) if h2>h1: print 'cant go in' return False, False final_hand_gap=max_gripper_width targetPositionList=[ [binMouth[0]-.15, sidepos, h1], [target_x, sidepos, h1], [target_x, sidepos, h2]] 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 'Plan number:',tp_index+1 if s: print 'Plan calculated successfully' plans.append(plan) else: print 'Plan calulation failed' return (False, False) qf = plan.q_traj[-1] #set robot speed setSpeedByName(speedName = 'faster') hand_gap=0 gripper.close() for numOfPlan in range(0, len(plans)): if isExecute: plans[numOfPlan].visualize(hand_param=hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() suction.start() gripper.set_force(10) gripper.grasp(move_pos=max_gripper_width) gripper.close() hand_gap=max_gripper_width print hand_gap ## retreat if num_iter==0: plan_offset=2 else: plan_offset=0 for numOfPlan in range(0, len(plans)-plan_offset): plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap) if isExecute: #backward_plan=plans[len(plans)-numOfPlan-1] #q_first=backward_plan[0] #q_last=backward_plan[len(backward_plan)-1] #if(abs(q_first[5]-q_last[5])>math.pi/2): # print 'something weird is going on with joint 5 rotation' # break pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() 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 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],qf=qf,num_iter=count) if suction_succeed: 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 qf plans=[] count=count+1 return (overall_plan_succeed,suction_succeed)
def manual_cal_1(binNum=0, strPos=None): if strPos == None: print 'No desired position entered' return ## 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 # 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) # 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(), 'tip', "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 distFromShelf = 0.05 mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor( distFromShelf, binNum) mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener) mouthBin = [ mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z ] # define modes here: # go home if strPos == 'home': print 'planning to go home' plan = Plan() plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] plan.visualize() raw_input('execute?') plan.execute() print 'done going home' # go mouth if strPos == 'mouth': print 'planning to go to mouth' 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?') plan.execute() if not s: print 'could not find plan' print 'done going to mouth' # go mid back bin if strPos == 'shelf': print 'planning to go to mid back of the bin' Orientation = [0.0, 0.7071, 0.0, 0.7071] targetPt = [mouthBin[0], 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() plan.visualize() s = plan.success() if s: raw_input('execute?') plan.execute() if not s: print 'could not find plan' print 'done going to mid back of the bin'
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], 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 suction_side(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=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 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 #tcp_offset_variables # plan store plans = [] ## 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) vertical_dim = object_dim[vertical_index] s1 = np.fabs(obj_pose_orient[1, 0]) * object_dim[0] s2 = np.fabs(obj_pose_orient[1, 1]) * object_dim[1] s3 = np.fabs(obj_pose_orient[1, 2]) * object_dim[2] s4 = np.fabs(obj_pose_orient[1, vertical_index]) * object_dim[vertical_index] horizontal_dim = s1 + s2 + s3 - s4 hand_gap = 0 gripper.close() while True: APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) < 6: continue else: 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 = 0.43 #change this to edit where you think the cup is 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 # for i in range(5): # 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='tip', 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 ] #this may cause issues!!! tcpPosHome = tcpPos # set scoop orientation (rotate wrist) 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 ] object_depth = objPosition[0] - binMouth[0] finger_length = .23 finger_width = .08 up_offset = .05 down_offset = .01 cup_to_spatula = .08 hand_width = .15 max_gripper_width = .110 hand_top_offset = hand_width / 2 + vert_offset + .015 hand_bot_offset = hand_width / 2 - vert_offset + .015 up_down_adjust = .025 side_offset = .03 binFloorHeight = binMouth[2] - bin_height / 2 binCeilHeight = binMouth[2] + bin_height / 2 min_height = binFloorHeight + lipHeight + finger_width / 2 + down_offset desired_height = objPosition[2] max_height = binCeilHeight - lipHeight - finger_width / 2 - down_offset - up_down_adjust target_height = desired_height if target_height < min_height: target_height = min_height if target_height > max_height: target_height = max_height bin_sideways = binMouth[1] sidepos = objPosition[1] horz_offset = 0.01 #this determines bin sides based on object #(binSmall,binLarge)=getSides(objPosition[1],listener) (binSmall, binLarge) = getSides(binNum, listener) binRight = binSmall binLeft = binLarge if sidepos > bin_sideways: #this stuff is what happens if the object is to the left print '[SucSide] Object is to the left' #turn the suction cup so that it is sideways left scoopOrientation = [.5, -.5, .5, -.5] side_waypoint1a = binRight + cup_to_spatula + .01 side_waypoint1b = sidepos - horizontal_dim / 2 - 2 * horz_offset if side_waypoint1a > side_waypoint1b: side_waypoint1 = side_waypoint1a print '[SucSide] side waypoint A' else: side_waypoint1 = side_waypoint1b print '[SucSide] side waypoint B' #side_waypoint2=sidepos+horizontal_dim-side_offset side_waypoint2 = binLeft - horizontal_dim - horz_offset if side_waypoint2 < side_waypoint1: if side_waypoint2 < side_waypoint1a: print '[SucSide] Not enought gap' return (False, False) else: side_waypoint1 = side_waypoint2 else: #this stuff is what happens if the object is to the right print '[SucSide] Object is to the right' #turn the suction cup so that it is sideways right scoopOrientation = [0.5, .5, .5, .5] side_waypoint1a = binLeft - cup_to_spatula - .01 side_waypoint1b = sidepos + horizontal_dim / 2 + 2 * horz_offset if side_waypoint1a < side_waypoint1b: side_waypoint1 = side_waypoint1a print '[SucSide] side waypoint A' else: side_waypoint1 = side_waypoint1b print '[SucSide] side waypoint B' #side_waypoint2=sidepos-horizontal_dim+side_offset side_waypoint2 = binRight + horizontal_dim + horz_offset if side_waypoint2 > side_waypoint1: if side_waypoint2 > side_waypoint1a: print '[SucSide] Not enought gap' return (False, False) else: side_waypoint1 = side_waypoint2 targetPositionList = [ [binMouth[0] - .15, side_waypoint1, target_height + up_down_adjust], [objPosition[0], side_waypoint1, target_height + up_down_adjust], [objPosition[0], side_waypoint2, target_height + up_down_adjust], [objPosition[0], side_waypoint2, target_height] ] qf = q0 for tp_index in range(0, len(targetPositionList)): targetPosition = targetPositionList[tp_index] frontOfObjectPtOutOfBin = targetPosition 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() if s: print '[SucSide] move to COM in y successful' print '[SucSide] tcp at:', targetPosition plan.visualize(hand_param=hand_gap) plans.append(plan) #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[SucSide] move to COM in y fail' return (False, False) #print plan.q_traj qf = plan.q_traj[-1] print '[SucSide] qf:', qf #set robot speed setSpeedByName(speedName='faster') for numOfPlan in range(0, len(plans)): if numOfPlan >= len(plans) - 2: setSpeedByName(speedName='fast') if isExecute: plans[numOfPlan].visualize(hand_param=hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() suction.start() final_hand_gap = 110 gripper.set_force(12) gripper.grasp(move_pos=final_hand_gap) gripper.close() hand_gap = final_hand_gap print '[SucSide] hand_gap:', hand_gap continue_suction = suction_items(objId) if continue_suction: print '[SucSide] object is of type that suction side will try to remove from bin' else: print '[SucSide] object is to big to remove from bin' ## retreat for numOfPlan in range(0, len(plans) - 1): plans[len(plans) - numOfPlan - 1].visualizeBackward(hand_param=hand_gap) if isExecute: pauseFunc(withPause) plans[len(plans) - numOfPlan - 1].executeBackward() if not continue_suction: suction.stop() rospy.sleep(3) print '[SucSide] Is suction in contact still? Lets see:' print '[SucSide] suction.check(): ', suction.check() print '[SucSide] suction.check(): ', suction.check() if suction.check(): #set robot speed print '[SucSide] got item. continuing. suction' setSpeedByName(speedName='fast') return (True, True) else: print '[SucSide] did not get item. Stopping suction' suction.stop() return (True, False)
def run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt): # hack to restart the script to prevent ros network issues. global pub limit = 100 cnt = 0 topics = ['-a'] # enumerate the possible trajectories for cnt_acc, acc in enumerate(accelerations): # enumerate the side we want to push for i in range(nside): # enumerate the contact point that we want to push for s in side_params: if shape_type == 'poly': pos = np.array(shape[i]) *(1-s) + np.array(shape[(i+1) % len(shape)]) *(s) pos = np.append(pos, [0]) tangent = np.array(shape[(i+1) % len(shape)]) - np.array(shape[i]) normal = np.array([tangent[1], -tangent[0]]) normal = normal / norm(normal) # normalize it normal = np.append(normal, [0]) elif shape_type == 'ellip': (a,b) = shape[0][0], shape[0][1] pos = [shape[0][0] * np.cos(s*2*np.pi), shape[0][1] * np.sin(s*2*np.pi), 0] normal = [np.cos(s*2*np.pi)/a, np.sin(s*2*np.pi)/b, 0] normal = normal / norm(normal) # normalize it elif shape_type == 'polyapprox': pos, normal = polyapprox(shape, s) # enumerate the direction in which we want to push for t in angles: for rep in xrange(nrep): if nrep > 1: bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep) else: bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t) bagfilepath = dir_save_bagfile+bagfilename # if exists then skip it if skip_when_exists and os.path.isfile(bagfilepath): #print bagfilepath, 'exits', 'skip' continue # find the probe pos in contact in object frame pos_probe_contact_object = pos + normal * probe_radius # find the start point direc = np.dot(tfm.euler_matrix(0,0,t) , normal.tolist() + [1])[0:3] # in the direction of moving out pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact if shape_type == 'polyapprox' and polyapprox_check_collision(shape, pos_start_probe_object, probe_radius): print bagfilename, 'will be in collision', 'skip' continue # find the end point pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact # zero force torque sensor rospy.sleep(0.1) setZero() wait_for_ft_calib() # transform start and end to world frame if hasRobot: pos_start_probe_world = coordinateFrameTransform(pos_start_probe_object, obj_frame_id, global_frame_id, listener) pos_end_probe_world = coordinateFrameTransform(pos_end_probe_object, obj_frame_id, global_frame_id, listener) pos_contact_probe_world = coordinateFrameTransform(pos_probe_contact_object, obj_frame_id, global_frame_id, listener) pos_center_obj_world = coordinateFrameTransform([0,0,0], obj_frame_id, global_frame_id, listener) else: pos_start_probe_world = pos_start_probe_object + center_world pos_end_probe_world = pos_end_probe_object + center_world pos_contact_probe_world = pos_probe_contact_object + center_world pos_center_obj_world = center_world # start bag recording # move to startPos setAcc(acc=globalhighacc, deacc=globalhighacc) setSpeed(tcp=global_highvel, ori=global_highvel) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = zup setCart(start_pos,ori) setAcc(acc=globalacc, deacc=globalacc) setSpeed(tcp=globalvel, ori=1000) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = z setCart(start_pos,ori) setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed mid_pos = copy.deepcopy(pos_contact_probe_world) mid_pos[2] = z pub.publish('move_to_mid_pos') setCart(mid_pos,ori) rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile) rospy.sleep(0.5) end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = z if acc == 0: # constant speed setAcc(acc=globalmaxacc, deacc=globalmaxacc) setSpeed(tcp=speeds[cnt_acc], ori=1000) else: # there is acceleration setAcc(acc=acc, deacc=globalmaxacc) setSpeed(tcp=1000, ori=1000) # some high speed pub.publish('start_pushing') setCart(end_pos,ori) pub.publish('end_pushing') # end bag recording helper.terminate_ros_node("/record") setSpeed(tcp=global_highvel, ori=1000) setAcc(acc=globalhighacc, deacc=globalhighacc) # move up vertically end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = zup setCart(end_pos,ori) distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world)) # recover recover(obj_slot, distance_obj_center > allowed_distance) #pause() cnt += 1 if cnt > limit: return
def manual_cal_1(binNum = 0, strPos = None): if strPos == None: print 'No desired position entered' return ## 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 # 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) # 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(), 'tip', "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 distFromShelf = 0.05 mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, binNum) mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener) mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z] # define modes here: # go home if strPos == 'home': print 'planning to go home' plan = Plan() plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] plan.visualize() raw_input('execute?') plan.execute() print 'done going home' # go mouth if strPos == 'mouth': print 'planning to go to mouth' 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?') plan.execute() if not s: print 'could not find plan' print 'done going to mouth' # go mid back bin if strPos == 'shelf': print 'planning to go to mid back of the bin' Orientation = [0.0, 0.7071, 0.0, 0.7071] targetPt = [mouthBin[0], 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() plan.visualize() s = plan.success() if s: raw_input('execute?') plan.execute() if not s: print 'could not find plan' print 'done going to mid back of the bin'
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 run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt): # hack to restart the script to prevent ros network issues. global pub limit = 100 cnt = 0 topics = ['-a'] # enumerate the possible trajectories for cnt_acc, acc in enumerate(accelerations): # enumerate the side we want to push for i in range(nside): # enumerate the contact point that we want to push for s in side_params: if shape_type == 'poly': pos = np.array(shape[i]) * (1 - s) + np.array(shape[ (i + 1) % len(shape)]) * (s) pos = np.append(pos, [0]) tangent = np.array(shape[(i + 1) % len(shape)]) - np.array( shape[i]) normal = np.array([tangent[1], -tangent[0]]) normal = normal / norm(normal) # normalize it normal = np.append(normal, [0]) elif shape_type == 'ellip': (a, b) = shape[0][0], shape[0][1] pos = [ shape[0][0] * np.cos(s * 2 * np.pi), shape[0][1] * np.sin(s * 2 * np.pi), 0 ] normal = [ np.cos(s * 2 * np.pi) / a, np.sin(s * 2 * np.pi) / b, 0 ] normal = normal / norm(normal) # normalize it elif shape_type == 'polyapprox': pos, normal = polyapprox(shape, s) # enumerate the direction in which we want to push for t in angles: for rep in xrange(nrep): if nrep > 1: bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % ( opt.surface_id, opt.shape_id, acc * 1000, speeds[cnt_acc], i, s, t, rep) else: bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % ( opt.surface_id, opt.shape_id, acc * 1000, speeds[cnt_acc], i, s, t) bagfilepath = dir_save_bagfile + bagfilename # if exists then skip it if skip_when_exists and os.path.isfile(bagfilepath): #print bagfilepath, 'exits', 'skip' continue # find the probe pos in contact in object frame pos_probe_contact_object = pos + normal * probe_radius # find the start point direc = np.dot( tfm.euler_matrix(0, 0, t), normal.tolist() + [1])[0:3] # in the direction of moving out pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact if shape_type == 'polyapprox' and polyapprox_check_collision( shape, pos_start_probe_object, probe_radius): print bagfilename, 'will be in collision', 'skip' continue # find the end point pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact # zero force torque sensor rospy.sleep(0.1) setZero() wait_for_ft_calib() # transform start and end to world frame if hasRobot: pos_start_probe_world = coordinateFrameTransform( pos_start_probe_object, obj_frame_id, global_frame_id, listener) pos_end_probe_world = coordinateFrameTransform( pos_end_probe_object, obj_frame_id, global_frame_id, listener) pos_contact_probe_world = coordinateFrameTransform( pos_probe_contact_object, obj_frame_id, global_frame_id, listener) pos_center_obj_world = coordinateFrameTransform( [0, 0, 0], obj_frame_id, global_frame_id, listener) else: pos_start_probe_world = pos_start_probe_object + center_world pos_end_probe_world = pos_end_probe_object + center_world pos_contact_probe_world = pos_probe_contact_object + center_world pos_center_obj_world = center_world # start bag recording # move to startPos setAcc(acc=globalhighacc, deacc=globalhighacc) setSpeed(tcp=global_highvel, ori=global_highvel) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = zup setCart(start_pos, ori) setAcc(acc=globalacc, deacc=globalacc) setSpeed(tcp=globalvel, ori=1000) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = z setCart(start_pos, ori) setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed mid_pos = copy.deepcopy(pos_contact_probe_world) mid_pos[2] = z pub.publish('move_to_mid_pos') setCart(mid_pos, ori) rosbag_proc = helper.start_ros_bag( bagfilename, topics, dir_save_bagfile) rospy.sleep(0.5) end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = z if acc == 0: # constant speed setAcc(acc=globalmaxacc, deacc=globalmaxacc) setSpeed(tcp=speeds[cnt_acc], ori=1000) else: # there is acceleration setAcc(acc=acc, deacc=globalmaxacc) setSpeed(tcp=1000, ori=1000) # some high speed pub.publish('start_pushing') setCart(end_pos, ori) pub.publish('end_pushing') # end bag recording helper.terminate_ros_node("/record") setSpeed(tcp=global_highvel, ori=1000) setAcc(acc=globalhighacc, deacc=globalhighacc) # move up vertically end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = zup setCart(end_pos, ori) distance_obj_center = np.linalg.norm( np.array(pos_center_obj_world) - np.array(center_world)) # recover recover(obj_slot, distance_obj_center > allowed_distance) #pause() cnt += 1 if cnt > limit: return
def main(argv=None): rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json' robot_touch_pts = readFromJson(filename) binNums = [0,2,3,5,6,8,9,11] nbin = len(binNums) diff_sum = 0 # shelf_pose/x: 1.92421 # shelf_pose/y: -0.0124050312627 # shelf_pose/z: -0.513553368384 ########################################### print 'floor of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'bin',i,'(expected - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum/nbin offset_x = diff_sum/nbin print 'Please move the shelf in z by', offset_x print '\twhich is', -0.513553368384 - offset_x ########################################### diff_sum_left = 0 print 'left wall of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = leftWall - touch_pt[0] # examine difference in x print 'left wall: bin',i,'(expected - robot_touch)=', diff diff_sum_left += diff print 'avg(diff) left wall=', diff_sum_left/nbin ########################################### diff_sum_right = 0 print 'right wall of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = rightWall - touch_pt[0] # examine difference in x print 'right wall: bin',i,'(expected - robot_touch)=', diff diff_sum_right += diff print 'avg(diff) right wall=', diff_sum_right/nbin offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2 print 'Please move the shelf in y by', offset_y print '\twhich is', 0.0124050312627 - offset_y ########################################### diff_sum = 0 print 'lip to floor of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'row',i,'(bin floor - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum/nbin offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2 print 'Please move the shelf in y by', offset_y print '\twhich is', 0.0124050312627 - offset_y ########################################## front diff_sum_front = 0 print 'front of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = rightWall - touch_pt[1] # examine difference in y print 'right front wall: bin',i,'(expected - robot_touch)=', diff diff_sum_front += diff print 'avg(diff) right wall=', diff_sum_front/nbin offset_x = diff_sum_front/nbin print 'Please move the shelf in x by', offset_x print '\twhich is', 1.92421 - offset_x #- 0.87/2
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 main(argv=None): rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib.json' robot_touch_floor_pts = readFromJson(filename) num_rows = 4 diff_sum = 0 print 'floor of the shelves' for i in range(num_rows): pose = coordinateFrameTransform(robot_touch_floor_pts[i], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'row',i,'(expected - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum/num_rows filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_left_side.json' robot_touch_left_pts = readFromJson(filename) num_rows = 4 diff_sum_left = 0 print 'left wall of the shelves' for i in range(num_rows): pose = coordinateFrameTransform(robot_touch_left_pts[i], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i*3) # a bin at row i diff = leftWall - touch_pt[0] # examine difference in x print 'left wall: row',i,'(expected - robot_touch)=', diff diff_sum_left += diff print 'avg(diff) left wall=', diff_sum_left/num_rows filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right_side.json' robot_touch_right_pts = readFromJson(filename) num_rows = 4 diff_sum_right = 0 print 'right wall of the shelves' for i in range(num_rows): pose = coordinateFrameTransform(robot_touch_right_pts[i], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i*3) # a bin at row i diff = rightWall - touch_pt[0] # examine difference in x print 'right wall: row',i,'(expected - robot_touch)=', diff diff_sum_right += diff print 'avg(diff) right wall=', diff_sum_right/num_rows filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right.json' robot_touch_lip_pts = readFromJson(filename) num_rows = 4 diff_sum = 0 print 'lip to floor of the shelves' for i in range(num_rows): pose = coordinateFrameTransform(robot_touch_lip_pts[i], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'row',i,'(bin floor - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum/num_rows
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 push_rotate(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1], binNum=4, objId='crayola_64_ct', bin_contents=None, robotConfig=None, grasp_range_lim=0.11, fing_width=0.06, isExecute=True, withPause=True): #~ ***************************************************************** obj_dim = get_obj_dim(objId) obj_dim = np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width = 0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.2) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion) shelf_pose_tfm = np.array(shelf_pose_tfm_list) shelf_pose_orient = shelf_pose_tfm[0:3, 0:3] #~ print 'shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos = shelf_pose_tfm[0:3, 3] #~ print 'shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0]) shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1]) shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame 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] obj_pose_pos = obj_pose_tfm[0:3, 3] #~ print 'obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0]) #~ print 'obj_X' #~ print obj_X obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1]) #~ print 'obj_Y' #~ print obj_Y obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2]) #~ print 'obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z)) obj_pose_orient_norm = obj_pose_orient_norm.transpose() #~ print 'obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm) #~ print 'proj' #~ print proj_vecY max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax( np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir] > 0: hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir] else: hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir] #~ print 'hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) * la.norm(hand_norm_vec)) angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180.0 / np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm = obj_dim[hand_norm_dir] print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX = np.dot(shelf_X, obj_pose_orient_norm) max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax( np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir] > 0: fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir] else: fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir] #~ print 'fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis = obj_dim[fing_axis_dir] print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm) max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax( np.fabs(proj_vecZ)) Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir] #~ print 'Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis = obj_dim[Zaxis_dir] hand_Y = np.cross(hand_norm_vec, fing_axis_vec) #~ ***************************************************************** #################### DECISION STRATEGIES ######################### bin_inner_cnstr = get_bin_inner_cnstr() proj_handNorm_ShelfX = np.dot(hand_norm_vec, shelf_X) right_push = False left_push = False # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis # # print 'proj_handNorm_ShelfX' # print proj_handNorm_ShelfX if obj_dim_along_hand_norm > obj_dim_along_fingAxis: #~ print 'proj_vecY' #~ print proj_vecY #~ print proj_vecY[hand_norm_dir] #~ #~ print 'proj_vecX' #~ print proj_vecX #~ print proj_vecX[fing_axis_dir] #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' #~ else: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' if proj_handNorm_ShelfX > 0: #Do left push Ypush_mult = 1 left_push = True print '[PushRotate] Left Push' else: #Do right push Ypush_mult = -1 print '[PushRotate] right Push' right_push = True push_offset = ( obj_dim_along_hand_norm / 2.0 ) #(Instead pf pushing on edge we will push 5 mm inside) else: #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' #~ else: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' if proj_handNorm_ShelfX > 0: #Do right push Ypush_mult = -1 print '[PushRotate] right Push' right_push = True else: #Do left push Ypush_mult = 1 print '[PushRotate] Left Push' left_push = True push_offset = (obj_dim_along_fingAxis / 2.0) num_intm_points = 5 push_x_intm = np.zeros(num_intm_points + 1) push_y_intm = np.zeros(num_intm_points + 1) backWall_shelf = bin_inner_cnstr[binNum][1] backWall_world = coordinateFrameTransform([0, backWall_shelf, 0], 'shelf', 'map', listener) back_check = backWall_world.pose.position.x - obj_dim_along_hand_norm / 2.0 binRightWall = bin_inner_cnstr[binNum][0] binLeftWall = bin_inner_cnstr[binNum][1] binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0], 'shelf', 'map', listener) binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf', 'map', listener) if left_push: sideWall_check = binRightWall_world.pose.position.y + ( fing_width / 2.0) + (obj_dim_along_fingAxis) if right_push: sideWall_check = binLeftWall_world.pose.position.y - ( fing_width / 2.0) - (obj_dim_along_fingAxis) for i in range(num_intm_points + 1): push_x_intm[i] = obj_pose_pos[0] + (push_offset) * np.sin( ((90 / num_intm_points) * i * np.pi) / 180.0) if push_x_intm[i] > back_check: push_x_intm[i] = back_check push_y_intm[i] = obj_pose_pos[1] + (Ypush_mult * push_offset) * np.cos( ((90 / num_intm_points) * i * np.pi) / 180.0) if left_push: if push_y_intm[i] < sideWall_check: push_y_intm[i] = sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' if right_push: if push_y_intm[i] > sideWall_check: push_y_intm[i] = sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' push_x_intm = np.array(push_x_intm) #~ print'push_x_intm' #~ print push_x_intm push_y_intm = np.array(push_y_intm) #******************************************************************* #Move the hand to the center of the bin and open the hand based on the height of the object bin_mid_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum) binFloorHeight_world = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf', 'map', listener) bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener) #******************************************************************* tcp_Z_off = 0.005 push_tcp_Z_shelfFrame = ( (bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) / 2.0) - tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform( [0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z push_z_intm = (push_tcp_Z) * np.ones(push_x_intm.size) push_series_pos = np.vstack((push_x_intm, push_y_intm, push_z_intm)) push_series_pos = push_series_pos.transpose() #~ print 'push_series_pos' #~ print push_series_pos #~ print push_series_pos[0,:] #~ print push_series_pos[-1,:] push_possible = True #****************************************************************** fing_push_pt = 0.2 * obj_dim_along_ZAxis + binFloorHeight_world.pose.position.z blade_tip_TCP_off = 0.038 # dist between finger inner end and spatual edge push_hand_opening = 2 * (push_tcp_Z - blade_tip_TCP_off - fing_push_pt) if push_hand_opening > grasp_range_lim: push_hand_opening = grasp_range_lim # Spatula finger should not hit the lip of the bin while pushing lip_Z_shelf = bin_inner_cnstr[binNum][4] lip_Z_world = coordinateFrameTransform([0, 0, lip_Z_shelf], 'shelf', 'map', listener) #~ lip_off=.025 #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off max_allowed_hand_opening_out = 2 * (push_tcp_Z - lip_Z_world.pose.position.z) max_allowed_hand_opening = max_allowed_hand_opening_out - 0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface if push_hand_opening > max_allowed_hand_opening: print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin' push_hand_opening = max_allowed_hand_opening #~ print 'push_hand_opening' #~ print push_hand_opening #~ #hand should not hit the side walls #~ binRightWall,binLeftWall=find_shelf_walls(binNum) #~ #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) side_wall_clearance = 0.0 print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y print '[PushRotate] right_hand_edge=', (push_y_intm[0] - fing_width - side_wall_clearance) print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y print '[PushRotate] left_hand_edge=', (push_y_intm[0] + fing_width + side_wall_clearance) if push_y_intm[ 0] - fing_width - side_wall_clearance < binRightWall_world.pose.position.y: print '[PushRotate] Hand will hit the right wall, Push rotate not possible' push_possible = False if push_y_intm[ 0] + fing_width + side_wall_clearance > binLeftWall_world.pose.position.y: print '[PushRotate] Hand will hit the left wall, push rotate not possible' push_possible = False #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient = deepcopy(spatula_down_orient) #~ ************************************************************* if push_possible == True: #Now we know where we wan to move TCP #Let's do robot motion planning now joint_topic = '/joint_states' # plan store plans = [] # set tcp (same as that set in generate dictionary) l1 = 0.0 l2 = 0.0 l3 = 0.47 tip_hand_transform = [ l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [ mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z ] gripperOri = [0, 0.7071, 0, 0.7071] pubFrame(br, pose=targetPosition + gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushRotate] move to bin mouth in push-rotate code successful' #plan.visualize() plans.append(plan) # Plan 0 #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[PushRotate] move to bin mouth in push-rotate code fail' return False qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ moveGripper(0.1,100) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1 = 0.0 #0.035 push_l2 = -Ypush_mult * ( fing_width / 2.0 ) # This should depend on the righ or left push conditions push_l3 = 0.47 push_tip_hand_transform = [ l1, l2, l3, 0, 0, 0 ] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) #~ q_initial = robotConfig # move just inside the bin with push orientation and open the hand suitable to push distFromShelf = -0.01 InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [ InbinPt.pose.position.x, push_series_pos[0, 1], InbinPt.pose.position.z ] #matching world_Y of first push pt print '[PushRotate] prepush_targetPosition=', prepush_targetPosition pubFrame(br, pose=prepush_targetPosition + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0=q_initial, target_tip_pos=prepush_targetPosition, target_tip_ori=push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] move inside bin in push orient successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] move inside bin in push orient fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH ############### # Open the gripper to dim calculated for pushing print '[PushRotate] hand opening to' print push_hand_opening * 1000.0 #~ moveGripper(push_hand_opening,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## #~ push_rotate_traj = Plan() #~ push_rotate_traj.q_traj = qf for i in range(0, push_x_intm.size): pushpt_pos = push_series_pos[i, :].transpose() pubFrame(br, pose=pushpt_pos.tolist() + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) #~ pdb.set_trace() # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient, # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb) planner = IK(q0=q_initial, target_tip_pos=pushpt_pos, target_tip_ori=push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] point inside push traj successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] point inside push traj fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* #~ if isExecute: #~ pauseFunc(withPause) #~ push_rotate_traj.execute() #################### CLOSE THE GRIPPER GRASP ################# #~ moveGripper(0.0,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[numOfPlan].visualize() pauseFunc(withPause) plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[len(plans) - numOfPlan - 1].visualizeBackward() pauseFunc(withPause) plans[len(plans) - numOfPlan - 1].executeBackward() #******************************************************************* print '[PushRotate] push_successful' return (push_possible, False)
def suction_side(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 = 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 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 #tcp_offset_variables # plan store plans = [] ## 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) vertical_dim=object_dim[vertical_index] s1=np.fabs(obj_pose_orient[1,0])*object_dim[0] s2=np.fabs(obj_pose_orient[1,1])*object_dim[1] s3=np.fabs(obj_pose_orient[1,2])*object_dim[2] s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index] horizontal_dim=s1+s2+s3-s4 hand_gap=0 gripper.close() while True: APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) < 6: continue else: 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 = 0.43 #change this to edit where you think the cup is 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 # for i in range(5): # 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='tip', 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] #this may cause issues!!! tcpPosHome = tcpPos # set scoop orientation (rotate wrist) 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] object_depth=objPosition[0]-binMouth[0] finger_length=.23 finger_width=.08 up_offset=.05 down_offset=.01 cup_to_spatula=.08 hand_width=.15 max_gripper_width=.110 hand_top_offset=hand_width/2+vert_offset+.015 hand_bot_offset=hand_width/2-vert_offset+.015 up_down_adjust=.025 side_offset=.03 binFloorHeight=binMouth[2]-bin_height/2 binCeilHeight=binMouth[2]+bin_height/2 min_height=binFloorHeight+lipHeight+finger_width/2+down_offset desired_height=objPosition[2] max_height=binCeilHeight-lipHeight-finger_width/2-down_offset-up_down_adjust target_height=desired_height if target_height<min_height: target_height=min_height if target_height>max_height: target_height=max_height bin_sideways=binMouth[1] sidepos=objPosition[1] horz_offset=0.01 #this determines bin sides based on object #(binSmall,binLarge)=getSides(objPosition[1],listener) (binSmall,binLarge)=getSides(binNum,listener) binRight=binSmall binLeft=binLarge if sidepos>bin_sideways: #this stuff is what happens if the object is to the left print '[SucSide] Object is to the left' #turn the suction cup so that it is sideways left scoopOrientation = [.5,-.5,.5,-.5] side_waypoint1a=binRight+cup_to_spatula+.01 side_waypoint1b=sidepos-horizontal_dim/2-2*horz_offset if side_waypoint1a>side_waypoint1b: side_waypoint1=side_waypoint1a print '[SucSide] side waypoint A' else: side_waypoint1=side_waypoint1b print '[SucSide] side waypoint B' #side_waypoint2=sidepos+horizontal_dim-side_offset side_waypoint2=binLeft-horizontal_dim-horz_offset if side_waypoint2<side_waypoint1: if side_waypoint2<side_waypoint1a: print '[SucSide] Not enought gap' return (False, False) else: side_waypoint1=side_waypoint2 else: #this stuff is what happens if the object is to the right print '[SucSide] Object is to the right' #turn the suction cup so that it is sideways right scoopOrientation = [0.5,.5,.5,.5] side_waypoint1a=binLeft-cup_to_spatula-.01 side_waypoint1b=sidepos+horizontal_dim/2+2*horz_offset if side_waypoint1a<side_waypoint1b: side_waypoint1=side_waypoint1a print '[SucSide] side waypoint A' else: side_waypoint1=side_waypoint1b print '[SucSide] side waypoint B' #side_waypoint2=sidepos-horizontal_dim+side_offset side_waypoint2=binRight+horizontal_dim+horz_offset if side_waypoint2>side_waypoint1: if side_waypoint2>side_waypoint1a: print '[SucSide] Not enought gap' return (False, False) else: side_waypoint1=side_waypoint2 targetPositionList=[ [binMouth[0]-.15, side_waypoint1, target_height+up_down_adjust], [objPosition[0], side_waypoint1, target_height+up_down_adjust], [objPosition[0], side_waypoint2, target_height+up_down_adjust], [objPosition[0], side_waypoint2, target_height]] qf=q0 for tp_index in range(0, len(targetPositionList)): targetPosition = targetPositionList[tp_index] frontOfObjectPtOutOfBin = targetPosition 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() if s: print '[SucSide] move to COM in y successful' print '[SucSide] tcp at:', targetPosition plan.visualize(hand_param=hand_gap) plans.append(plan) #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[SucSide] move to COM in y fail' return (False, False) #print plan.q_traj qf = plan.q_traj[-1] print '[SucSide] qf:', qf #set robot speed setSpeedByName(speedName = 'faster') for numOfPlan in range(0, len(plans)): if numOfPlan >=len(plans)-2: setSpeedByName(speedName = 'fast') if isExecute: plans[numOfPlan].visualize(hand_param=hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() suction.start() final_hand_gap=110 gripper.set_force(12) gripper.grasp(move_pos=final_hand_gap) gripper.close() hand_gap=final_hand_gap print '[SucSide] hand_gap:', hand_gap continue_suction=suction_items(objId) if continue_suction: print '[SucSide] object is of type that suction side will try to remove from bin' else: print '[SucSide] object is to big to remove from bin' ## retreat for numOfPlan in range(0, len(plans)-1): plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap) if isExecute: pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() if not continue_suction: suction.stop() rospy.sleep(3) print '[SucSide] Is suction in contact still? Lets see:' print '[SucSide] suction.check(): ', suction.check() print '[SucSide] suction.check(): ', suction.check() if suction.check(): #set robot speed print '[SucSide] got item. continuing. suction' setSpeedByName(speedName = 'fast') return (True, True) else: print '[SucSide] did not get item. Stopping suction' suction.stop() return (True, False)
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 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 main(argv=None): rospy.init_node('calib', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) filename = os.environ[ 'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json' robot_touch_pts = readFromJson(filename) binNums = [0, 2, 3, 5, 6, 8, 9, 11] nbin = len(binNums) diff_sum = 0 # shelf_pose/x: 1.92421 # shelf_pose/y: -0.0124050312627 # shelf_pose/z: -0.513553368384 ########################################### print 'floor of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'bin', i, '(expected - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum / nbin offset_x = diff_sum / nbin print 'Please move the shelf in z by', offset_x print '\twhich is', -0.513553368384 - offset_x ########################################### diff_sum_left = 0 print 'left wall of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = leftWall - touch_pt[0] # examine difference in x print 'left wall: bin', i, '(expected - robot_touch)=', diff diff_sum_left += diff print 'avg(diff) left wall=', diff_sum_left / nbin ########################################### diff_sum_right = 0 print 'right wall of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = rightWall - touch_pt[0] # examine difference in x print 'right wall: bin', i, '(expected - robot_touch)=', diff diff_sum_right += diff print 'avg(diff) right wall=', diff_sum_right / nbin offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2 print 'Please move the shelf in y by', offset_y print '\twhich is', 0.0124050312627 - offset_y ########################################### diff_sum = 0 print 'lip to floor of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i diff = binFloorHeight - touch_pt[2] # examine difference in z print 'row', i, '(bin floor - robot_touch)=', diff diff_sum += diff print 'avg(diff)=', diff_sum / nbin offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2 print 'Please move the shelf in y by', offset_y print '\twhich is', 0.0124050312627 - offset_y ########################################## front diff_sum_front = 0 print 'front of the shelves' for i in binNums: pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener) touch_pt = pose2list(pose.pose) (leftWall, rightWall) = getBinSideWalls(i) # a bin at row i diff = rightWall - touch_pt[1] # examine difference in y print 'right front wall: bin', i, '(expected - robot_touch)=', diff diff_sum_front += diff print 'avg(diff) right wall=', diff_sum_front / nbin offset_x = diff_sum_front / nbin print 'Please move the shelf in x by', offset_x print '\twhich is', 1.92421 - offset_x #- 0.87/2
def flush_grasp(objPose = [1.95,1.25,1.4,0,0,0,1], binNum=4, objId = 0, bin_contents = None, robotConfig = None, forceThreshold = 1, isExecute = True, withPause = 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 joint_topic = '/joint_states' ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.1) # shelf variables pretensionDelta = 0.00 lipHeight = 0.035 #tcp_offset_variables #set robot speed setSpeedByName(speedName = 'fast') # plan store plans = [] ## 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) vertical_dim=object_dim[vertical_index] s1=np.fabs(obj_pose_orient[1,0])*object_dim[0] s2=np.fabs(obj_pose_orient[1,1])*object_dim[1] s3=np.fabs(obj_pose_orient[1,2])*object_dim[2] s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index] horizontal_dim=s1+s2+s3-s4 hand_gap=0 gripper.close() while True: APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) < 6: continue else: 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 = 0.43 #change this to edit where you think the cup is 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 # for i in range(5): # 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='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) 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] object_depth=objPosition[0]-binMouth[0] finger_length=.23 finger_width=.06 up_offset=.05 down_offset=0.005 cup_to_spatula=.08 hand_width=.15 max_gripper_width=.110 hand_top_offset=hand_width/2+vert_offset+.03 hand_bot_offset=hand_width/2-vert_offset+.015 side_offset=.03 binFloorHeight=binMouth[2]-bin_height/2 binCeilHeight=binMouth[2]+bin_height/2 min_height=binFloorHeight+lipHeight+finger_width/2+down_offset desired_height=objPosition[2] max_height=binCeilHeight-lipHeight-finger_width/2-down_offset target_height=desired_height if target_height<min_height: target_height=min_height if target_height>max_height: target_height=max_height bin_sideways=binMouth[1] sidepos=objPosition[1] horz_offset=0.015 #this determines bin sides based on bin input binSmall=binMouth[1]-bin_width/2 binLarge=binMouth[1]+bin_width/2 #this determines bin sides based on object #(binSmall,binLarge)=getSides(objPosition[1],listener) binRight=binSmall binLeft=binLarge final_hand_gap=110 if sidepos>bin_sideways: #this stuff is what happens if the object is to the left print 'Object is to the left' if binNum==0 or binNum==3 or binNum==6 or binNum==9: print 'Object is in that weird rail corner' return False #turn the suction cup so that it is sideways left #scoopOrientation = [.5,-.5,.5,-.5] scoopOrientation = [0.5,.5,.5,.5] #side_waypoint1=binRight+cup_to_spatula+.01 #side_waypoint2=sidepos+horizontal_dim-side_offset #side_waypoint2=binLeft-horizontal_dim-horz_offset side_waypoint1=binLeft-hand_top_offset+horz_offset if sidepos-horizontal_dim<binLeft-final_hand_gap: print 'this is not a flush object' return False else: #this stuff is what happens if the object is to the right print 'Object is to the right' if binNum==2 or binNum==5 or binNum==8 or binNum==11: print 'Object is in that weird rail corner' return False #turn the suction cup so that it is sideways right #scoopOrientation = [0.5,.5,.5,.5] scoopOrientation = [.5,-.5,.5,-.5] #side_waypoint1=binLeft-cup_to_spatula-.01 #side_waypoint2=sidepos-horizontal_dim+side_offset #side_waypoint2=binRight+horizontal_dim+horz_offset side_waypoint1=binRight+hand_top_offset-horz_offset if sidepos+horizontal_dim>binRight+final_hand_gap: print 'this is not a flush object' return False targetPositionList=[ [binMouth[0]-.15, side_waypoint1, min_height], [binMouth[0]+.04, side_waypoint1, min_height], [binMouth[0]+.3, side_waypoint1, min_height], [binMouth[0]+.3, side_waypoint1, min_height+.03], [binMouth[0]-.1, side_waypoint1, min_height+.03]] qf=q0 for tp_index in range(0, len(targetPositionList)): targetPosition = targetPositionList[tp_index] frontOfObjectPtOutOfBin = targetPosition 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() if s: print 'move to COM in y successful' print 'tcp at:' print(targetPosition) #plan.visualize(hand_param=hand_gap) plans.append(plan) #if isExecute: # pauseFunc(withPause) # plan.execute() else: print 'move to COM in y fail' return False #print plan.q_traj qf = plan.q_traj[-1] print qf for numOfPlan in range(0, 2): if isExecute: plans[numOfPlan].visualize(hand_param=final_hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() #time.sleep(2.5) #suction.start() final_hand_gap=110 gripper.set_force(20) gripper.grasp(move_pos=final_hand_gap) for numOfPlan in range(2, 3): if isExecute: plans[numOfPlan].visualize(hand_param=hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() gripper.set_force(50) rospy.sleep(0.1) gripper.move(move_pos=0) hand_gap=final_hand_gap print hand_gap ## retreat #for numOfPlan in range(0, len(plans)-1): # plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap) # if isExecute: # pauseFunc(withPause) # plans[len(plans)-numOfPlan-1].executeBackward() # suction.stop() for numOfPlan in range(3, len(plans)): if isExecute: plans[numOfPlan].visualize(hand_param=hand_gap) pauseFunc(withPause) plans[numOfPlan].execute() return True
def push_back(objPose=[1.95,0.25,1.4,0,0,0,1], binNum=4, objId = 'crayola_64_ct', bin_contents = ['paper_mate_12_count_mirado_black_warrior','crayola_64_ct','expo_dry_erase_board_eraser'], robotConfig = None, grasp_range_lim=0.11, fing_width=0.06, isExecute = True, withPause = True): #~ ***************************************************************** obj_dim=get_obj_dim(objId) obj_dim=np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width=0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion) shelf_pose_tfm=np.array(shelf_pose_tfm_list) shelf_pose_orient=shelf_pose_tfm[0:3,0:3] #~ print '[PushBack] shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos=shelf_pose_tfm[0:3,3] #~ print '[PushBack] shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0]) shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1]) shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame 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] obj_pose_pos=obj_pose_tfm[0:3,3] #~ print '[PushBack] obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0]) #~ print '[PushBack] obj_X' #~ print obj_X obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1]) #~ print '[PushBack] obj_Y' #~ print obj_Y obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2]) #~ print '[PushBack] obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z)) obj_pose_orient_norm=obj_pose_orient_norm.transpose() #~ print '[PushBack] obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm) #~ print '[PushBack] proj' #~ print proj_vecY max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir]>0: hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir] else: hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir] #~ print '[PushBack] hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec)); angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180/np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm=obj_dim[hand_norm_dir] # print '[PushBack] dim along hand norm' # print obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX=np.dot(shelf_X,obj_pose_orient_norm) max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir]>0: fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir] else: fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir] #~ print '[PushBack] fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis=obj_dim[fing_axis_dir] # print '[PushBack] obj_dim_along_fingAxis' # print obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm) max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ)) Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir] #~ print '[PushBack] Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis=obj_dim[Zaxis_dir] hand_Y=np.cross(hand_norm_vec,fing_axis_vec) diag_dist=la.norm(np.array([obj_dim_along_hand_norm,obj_dim_along_fingAxis])) # print'req obj diag dist=', diag_dist #~ ***************************************************************** # Find the maximum distance to which we will push other_diag=np.zeros(len(bin_contents)) for num_bin_contents in range(0, len(bin_contents)): if bin_contents[num_bin_contents] != objId: temp_obj_dim=get_obj_dim(objId) other_diag[num_bin_contents]=la.norm(np.array([temp_obj_dim[0],temp_obj_dim[1]])) max_diag,max_obj_ID=np.max(np.fabs(other_diag)), np.argmax(np.fabs(other_diag)) # print '[PushBack] max_obj_ID',max_diag # print '[PushBack] max_obj_ID', bin_contents[max_obj_ID] #******************************************************************* bin_inner_cnstr=get_bin_inner_cnstr() #******************************************************************* #Check if we can do left push diag_factor=0.75 obj_left_edge=obj_pose_pos[1]+diag_factor*diag_dist/2.0 binLeftWall=bin_inner_cnstr[binNum][1] binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) binRightWall=bin_inner_cnstr[binNum][0] binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) left_gap=binLeftWall_world.pose.position.y-obj_left_edge # print '[PushBack] left_gap=', left_gap fing_clearance=0.010 # It's like 5 mm on both sides of finger bin_width=binLeftWall_world.pose.position.y-binRightWall_world.pose.position.y tolerance_in_push=0.020 if left_gap+diag_dist/2.0-tolerance_in_push>bin_width: print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin' return (False,False) if left_gap > fing_width+fing_clearance: left_push=True print '[PushBack] left push is possible' else: left_push=False print '[PushBack] left push is NOT possible' # check if we can do right push obj_right_edge=obj_pose_pos[1]-diag_factor*diag_dist/2.0 right_gap=obj_right_edge-binRightWall_world.pose.position.y # print '[PushBack] right_gap', right_gap fing_clearance=0.010 # It's like 5 mm on both sides of finger if right_gap+diag_dist/2.0-tolerance_in_push>bin_width: print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin' return (False,False) if right_gap > fing_width+fing_clearance: right_push=True print '[PushBack] right push is possible' else: right_push=False print '[PushBack] right push is NOT possible' #******************************************************************* #Find Push Points for left push bin_face_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum) bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map', listener) tcp_Z_off=0.005 #lower down from center of the bin push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z push_side_off=1.0 if left_push: left_pushPt_X=bin_face_pos_world.pose.position.x+0.05 left_pushPt_Y=binLeftWall_world.pose.position.y-push_side_off*left_gap # left_Y_lim=binLeftWall_world.pose.position.y- left_pushPt_Z=deepcopy(push_tcp_Z) left_push_Pt=np.array([left_pushPt_X,left_pushPt_Y,left_pushPt_Z]) print '[PushBack] expected left push clearance=',binLeftWall_world.pose.position.y-left_pushPt_Y-fing_width/2.0 #Find Push Points for right push if right_push: right_pushPt_X=bin_face_pos_world.pose.position.x+0.05 right_pushPt_Y=binRightWall_world.pose.position.y+push_side_off*right_gap right_pushPt_Z=deepcopy(push_tcp_Z) right_push_Pt=np.array([right_pushPt_X,right_pushPt_Y,right_pushPt_Z]) print '[PushBack] expected right push clearance=',right_pushPt_Y-binRightWall_world.pose.position.y-fing_width/2.0 #Execute push trajectories #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient=spatula_down_orient push_hand_opening=0.100 joint_topic = '/joint_states' #~ ************************************************************* #Execure left push trajectory if left_push: # plan store left_plans = [] # set tcp (same as that set in generate dictionary) l1=0.0 l2=0.0 l3 = 0.47 tip_hand_transform = [l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z] gripperOri=[0, 0.7071, 0, 0.7071] pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushBack] move to bin mouth in push-back code successful (left push)' left_plans.append(plan) # Plan 0 else: print '[PushBack] move to bin mouth in push-back code fail (left push)' return (False,False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) left_plans.append(grasp_plan) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1=0.0 push_l2=0.0 push_l3 = 0.47 push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) distFromShelf = -0.015 InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [InbinPt.pose.position.x, left_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] move inside bin in push orient successful' #~ print '[PushBack] tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() left_plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushBack] move inside bin in push orient fail' return (False,False) qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH############### # Open the gripper to dim calculated for pushing print '[PushBack] hand opening to' print push_hand_opening*1000.0 grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) left_plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## back_bin_X=bin_face_pos_world.pose.position.x+0.43 push_stop_worldX=back_bin_X-max_diag-0.010 left_pushStop_Pt_pos=deepcopy(left_push_Pt) left_pushStop_Pt_pos[0]=push_stop_worldX #Push until the end planner = IK(q0 = q_initial, target_tip_pos = left_pushStop_Pt_pos, target_tip_ori = push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] start point push traj successful' left_plans.append(plan) # Plan 1 else: print '[PushBack] start point push traj fail' return (False,False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) left_plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(left_plans)): if isExecute: left_plans[numOfPlan].visualize() pauseFunc(withPause) left_plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(left_plans)): if isExecute: left_plans[numOfPlan].visualizeBackward() pauseFunc(withPause) left_plans[len(left_plans)-numOfPlan-1].executeBackward() #******************************************************************* if right_push: # plan store right_plans = [] # set tcp (same as that set in generate dictionary) l1=0.0 l2=0.0 l3 = 0.47 tip_hand_transform = [l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z] gripperOri=[0, 0.7071, 0, 0.7071] pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushBack] move to bin mouth in push-back code successful (right push)' right_plans.append(plan) # Plan 0 else: print '[PushBack] move to bin mouth in push-back code fail (right push)' return (False,False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) right_plans.append(grasp_plan) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1=0.0 push_l2=0.0 push_l3 = 0.47 push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) distFromShelf = -0.015 InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [InbinPt.pose.position.x, right_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] move inside bin in push orient successful' #~ print '[PushBack] tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() right_plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushBack] move inside bin in push orient fail' return (False,False) qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH############### # Open the gripper to dim calculated for pushing print '[PushBack] hand opening to' print push_hand_opening*1000.0 grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) right_plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## back_bin_X=bin_face_pos_world.pose.position.x+0.42 push_stop_worldX=back_bin_X-max_diag-0.010 right_pushStop_Pt_pos=deepcopy(right_push_Pt) right_pushStop_Pt_pos[0]=push_stop_worldX #Push until the end planner = IK(q0 = q_initial, target_tip_pos = right_pushStop_Pt_pos, target_tip_ori = push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] start point push traj successful' right_plans.append(plan) # Plan 1 else: print '[PushBack] start point push traj fail' return (False,False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) right_plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(right_plans)): if isExecute: right_plans[numOfPlan].visualize() pauseFunc(withPause) right_plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(right_plans)): if isExecute: right_plans[numOfPlan].visualizeBackward() pauseFunc(withPause) right_plans[len(right_plans)-numOfPlan-1].executeBackward() if right_push or left_push: push_possible=True else: push_possible=False return(push_possible, False)
def manual_cal_2(binNum=0, endBinNum=11): ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) joint_topic = '/joint_states' # set tcp l1 = 0.06345 l2 = 0.400 tip_hand_transform = [ l1, 0, l2, 0, 0, 0 ] # to be updated when we have a hand design finalized moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more) moveDistVertical = 0.08 #we could do 1 more cm #set Speed ik.ik.setSpeedByName('faster') # broadcast frame attached to tcp #~ rospy.sleep(0.1) #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6") #~ rospy.sleep(0.1) # get position of the tcp in world frame pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) tcpPos = [ pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z ] tcpPosHome = tcpPos orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist) #~ rospy.sleep(0.1) #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6") #~ rospy.sleep(0.1) pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) tcpPos = [ pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z ] print 'TCP is at:', tcpPos print 'planning to go home' # home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463] # planner = IKJoint(target_joint_pos=home_joint_pose) # plan = planner.plan() # plan.visualize() raw_input('execute?') # plan.execute() goToHome.goToHome() print 'done going home' calibBins = [] for x in range(binNum, endBinNum + 1): calibPts = [] if x == 1 or x == 4 or x == 7 or x == 10: calibBins.append([]) continue distFromShelf = 0.05 mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor( distFromShelf, x) mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener) mouthBin = [ mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z ] epsilon = 0.016 distFromShelf = distFromShelf - epsilon # go to mouth of bin print 'planning to go to mouth but a little out' Orientation = [0.0, 0.7071, 0.0, 0.7071] targetPt = mouthBin planner = IK(q0=None, target_tip_pos=targetPt, target_tip_ori=Orientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.visualize() s = plan.success() if s: raw_input('execute?') ik.ik.setSpeedByName('fast') plan.execute() if not s: print 'could not find plan' print 'done going to mouth' # go to the mouth of the bin but actually inside print 'planning to go to start position' Orientation = [0.0, 0.7071, 0.0, 0.7071] targetPt = [mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2]] planner = IK(q0=None, target_tip_pos=targetPt, target_tip_ori=Orientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() if s: plan.visualize() raw_input('execute?') ik.ik.setSpeedByName('fast') plan.execute() if not s: print 'could not find plan' print 'done going to start position' OrientationList = [] targetPtList = [] captionList = [] ntouch = 5 # go down captionList.append('go down') Orientation = [0.0, 0.7071, 0.0, 0.7071] targetPt = [ mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2] - moveDistVertical ] OrientationList.append(Orientation) targetPtList.append(targetPt) # go left captionList.append('go left') Orientation = [0.5, 0.5, 0.5, 0.5] targetPt = [ mouthBin[0] + distFromShelf, mouthBin[1] + moveDistHorizontal, mouthBin[2] ] OrientationList.append(Orientation) targetPtList.append(targetPt) # go right captionList.append('go right') Orientation = [0.5, -0.5, 0.5, -0.5] targetPt = [ mouthBin[0] + distFromShelf, mouthBin[1] - moveDistHorizontal, mouthBin[2] ] OrientationList.append(Orientation) targetPtList.append(targetPt) # go up captionList.append('go up') Orientation = [0.7071, 0, 0.7071, 0] targetPt = [ mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2] + moveDistVertical ] OrientationList.append(Orientation) targetPtList.append(targetPt) # go to mid back of the bin captionList.append('go mid back') Orientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4] targetPt = [mouthBin[0] + 0.30, mouthBin[1], mouthBin[2] - 0.08] OrientationList.append(Orientation) targetPtList.append(targetPt) for touchInd in range(ntouch): print captionList[touchInd] targetPt = targetPtList[touchInd] Orientation = OrientationList[touchInd] planner = IK(q0=None, target_tip_pos=targetPt, target_tip_ori=Orientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() if s: plan.visualize() raw_input('execute?') ik.ik.setSpeedByName('fast') plan.execute() if not s: print 'could not find plan' print 'done going down, TELEOP TIME:' raw_input('when done with teleop hit enter: ') #~ rospy.sleep(0.1) #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6") #~ rospy.sleep(0.1) t = listener.getLatestCommonTime('calib', 'map') (calibPoseTrans, calibPoseRot) = listener.lookupTransform('map', 'calib', t) calibPose = list(calibPoseTrans) + list(calibPoseRot) #~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) #~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z] calibPts.append(calibPose) raw_input('done recording position, hit enter to go back') if s: plan.executeBackward() else: print 'Please teleop back' raw_input('done?') calibBins.append(calibPts) filename = os.environ[ 'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json' with open(filename, 'w') as outfile: json.dump(calibBins, outfile) print 'planning to go home' home_joint_pose = [ -0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463 ] planner = IKJoint(target_joint_pos=home_joint_pose) plan = planner.plan() plan.visualize() raw_input('execute?') ik.ik.setSpeedByName('faster') plan.execute() print 'done going home' print 'dumped to file ', filename
def 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 push_rotate(objPose=[1.95,0.25,1.4,0,0,0,1], binNum=4, objId = 'crayola_64_ct', bin_contents = None, robotConfig = None, grasp_range_lim=0.11, fing_width=0.06, isExecute = True, withPause = True): #~ ***************************************************************** obj_dim=get_obj_dim(objId) obj_dim=np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width=0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.2) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion) shelf_pose_tfm=np.array(shelf_pose_tfm_list) shelf_pose_orient=shelf_pose_tfm[0:3,0:3] #~ print 'shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos=shelf_pose_tfm[0:3,3] #~ print 'shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0]) shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1]) shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame 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] obj_pose_pos=obj_pose_tfm[0:3,3] #~ print 'obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0]) #~ print 'obj_X' #~ print obj_X obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1]) #~ print 'obj_Y' #~ print obj_Y obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2]) #~ print 'obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z)) obj_pose_orient_norm=obj_pose_orient_norm.transpose() #~ print 'obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm) #~ print 'proj' #~ print proj_vecY max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir]>0: hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir] else: hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir] #~ print 'hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec)); angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180.0/np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm=obj_dim[hand_norm_dir] print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX=np.dot(shelf_X,obj_pose_orient_norm) max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir]>0: fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir] else: fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir] #~ print 'fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis=obj_dim[fing_axis_dir] print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm) max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ)) Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir] #~ print 'Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis=obj_dim[Zaxis_dir] hand_Y=np.cross(hand_norm_vec,fing_axis_vec) #~ ***************************************************************** #################### DECISION STRATEGIES ######################### bin_inner_cnstr=get_bin_inner_cnstr() proj_handNorm_ShelfX=np.dot(hand_norm_vec,shelf_X) right_push=False left_push=False # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis # # print 'proj_handNorm_ShelfX' # print proj_handNorm_ShelfX if obj_dim_along_hand_norm>obj_dim_along_fingAxis: #~ print 'proj_vecY' #~ print proj_vecY #~ print proj_vecY[hand_norm_dir] #~ #~ print 'proj_vecX' #~ print proj_vecX #~ print proj_vecX[fing_axis_dir] #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' #~ else: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' if proj_handNorm_ShelfX>0: #Do left push Ypush_mult=1 left_push=True print '[PushRotate] Left Push' else: #Do right push Ypush_mult=-1 print '[PushRotate] right Push' right_push=True push_offset=(obj_dim_along_hand_norm/2.0) #(Instead pf pushing on edge we will push 5 mm inside) else: #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0: #~ #Do right push #~ Ypush_mult=-1 #~ print 'right Push' #~ else: #~ #Do left push #~ Ypush_mult=1 #~ print 'Left Push' if proj_handNorm_ShelfX>0: #Do right push Ypush_mult=-1 print '[PushRotate] right Push' right_push=True else: #Do left push Ypush_mult=1 print '[PushRotate] Left Push' left_push=True push_offset=(obj_dim_along_fingAxis/2.0) num_intm_points=5 push_x_intm=np.zeros(num_intm_points+1) push_y_intm=np.zeros(num_intm_points+1) backWall_shelf=bin_inner_cnstr[binNum][1] backWall_world = coordinateFrameTransform([0,backWall_shelf,0], 'shelf', 'map', listener) back_check=backWall_world.pose.position.x-obj_dim_along_hand_norm/2.0 binRightWall=bin_inner_cnstr[binNum][0] binLeftWall=bin_inner_cnstr[binNum][1] binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) if left_push: sideWall_check=binRightWall_world.pose.position.y+(fing_width/2.0)+(obj_dim_along_fingAxis) if right_push: sideWall_check=binLeftWall_world.pose.position.y-(fing_width/2.0)-(obj_dim_along_fingAxis) for i in range(num_intm_points+1): push_x_intm[i]=obj_pose_pos[0]+(push_offset)*np.sin(((90/num_intm_points)*i*np.pi)/180.0) if push_x_intm[i]>back_check: push_x_intm[i]=back_check push_y_intm[i]=obj_pose_pos[1]+(Ypush_mult*push_offset)*np.cos(((90/num_intm_points)*i*np.pi)/180.0) if left_push: if push_y_intm[i]<sideWall_check: push_y_intm[i]=sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' if right_push: if push_y_intm[i]>sideWall_check: push_y_intm[i]=sideWall_check print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper' push_x_intm=np.array(push_x_intm) #~ print'push_x_intm' #~ print push_x_intm push_y_intm=np.array(push_y_intm) #******************************************************************* #Move the hand to the center of the bin and open the hand based on the height of the object bin_mid_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum) binFloorHeight_world = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener) bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener) #******************************************************************* tcp_Z_off=0.005 push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z push_z_intm=(push_tcp_Z)*np.ones(push_x_intm.size) push_series_pos=np.vstack((push_x_intm,push_y_intm,push_z_intm)) push_series_pos=push_series_pos.transpose() #~ print 'push_series_pos' #~ print push_series_pos #~ print push_series_pos[0,:] #~ print push_series_pos[-1,:] push_possible=True #****************************************************************** fing_push_pt=0.2*obj_dim_along_ZAxis+binFloorHeight_world.pose.position.z blade_tip_TCP_off=0.038 # dist between finger inner end and spatual edge push_hand_opening=2*(push_tcp_Z-blade_tip_TCP_off-fing_push_pt) if push_hand_opening>grasp_range_lim: push_hand_opening=grasp_range_lim # Spatula finger should not hit the lip of the bin while pushing lip_Z_shelf=bin_inner_cnstr[binNum][4] lip_Z_world=coordinateFrameTransform([0,0,lip_Z_shelf], 'shelf', 'map', listener) #~ lip_off=.025 #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off max_allowed_hand_opening_out=2*(push_tcp_Z-lip_Z_world.pose.position.z) max_allowed_hand_opening=max_allowed_hand_opening_out-0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface if push_hand_opening>max_allowed_hand_opening: print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin' push_hand_opening=max_allowed_hand_opening #~ print 'push_hand_opening' #~ print push_hand_opening #~ #hand should not hit the side walls #~ binRightWall,binLeftWall=find_shelf_walls(binNum) #~ #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener) #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener) side_wall_clearance=0.0 print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y print '[PushRotate] right_hand_edge=', (push_y_intm[0]-fing_width-side_wall_clearance) print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y print '[PushRotate] left_hand_edge=', (push_y_intm[0]+fing_width+side_wall_clearance) if push_y_intm[0]-fing_width-side_wall_clearance<binRightWall_world.pose.position.y: print '[PushRotate] Hand will hit the right wall, Push rotate not possible' push_possible=False if push_y_intm[0]+fing_width+side_wall_clearance>binLeftWall_world.pose.position.y: print '[PushRotate] Hand will hit the left wall, push rotate not possible' push_possible=False #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient=deepcopy(spatula_down_orient) #~ ************************************************************* if push_possible==True: #Now we know where we wan to move TCP #Let's do robot motion planning now joint_topic = '/joint_states' # plan store plans = [] # set tcp (same as that set in generate dictionary) l1=0.0 l2=0.0 l3 = 0.47 tip_hand_transform = [l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z] gripperOri=[0, 0.7071, 0, 0.7071] pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushRotate] move to bin mouth in push-rotate code successful' #plan.visualize() plans.append(plan) # Plan 0 #if isExecute: # pauseFunc(withPause) # plan.execute() else: print '[PushRotate] move to bin mouth in push-rotate code fail' return False qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ moveGripper(0.1,100) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1=0.0 #0.035 push_l2=-Ypush_mult*(fing_width/2.0) # This should depend on the righ or left push conditions push_l3 = 0.47 push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) #~ q_initial = robotConfig # move just inside the bin with push orientation and open the hand suitable to push distFromShelf = -0.01 InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [InbinPt.pose.position.x, push_series_pos[0,1] , InbinPt.pose.position.z] #matching world_Y of first push pt print '[PushRotate] prepush_targetPosition=', prepush_targetPosition pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] move inside bin in push orient successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] move inside bin in push orient fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH ############### # Open the gripper to dim calculated for pushing print '[PushRotate] hand opening to' print push_hand_opening*1000.0 #~ moveGripper(push_hand_opening,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## #~ push_rotate_traj = Plan() #~ push_rotate_traj.q_traj = qf for i in range(0,push_x_intm.size): pushpt_pos=push_series_pos[i,:].transpose() pubFrame(br, pose=pushpt_pos.tolist()+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) #~ pdb.set_trace() # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient, # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb) planner = IK(q0 = q_initial, target_tip_pos = pushpt_pos, target_tip_ori = push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('slow') s = plan.success() if s: print '[PushRotate] point inside push traj successful' #~ print 'tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushRotate] point inside push traj fail' return False qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* #~ if isExecute: #~ pauseFunc(withPause) #~ push_rotate_traj.execute() #################### CLOSE THE GRIPPER GRASP ################# #~ moveGripper(0.0,100) grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[numOfPlan].visualize() pauseFunc(withPause) plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(plans)): if isExecute: plans[len(plans)-numOfPlan-1].visualizeBackward() pauseFunc(withPause) plans[len(plans)-numOfPlan-1].executeBackward() #******************************************************************* print '[PushRotate] push_successful' return (push_possible,False)
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 main(argv): # prepare the proxy, listener global listener global vizpub rospy.init_node('collect_motion_data') listener = tf.TransformListener() vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10) br = TransformBroadcaster() parser = optparse.OptionParser() parser.add_option('-s', action="store", dest='shape_id', help='The shape id e.g. rect1-rect3', default='rect1') parser.add_option('', '--surface', action="store", dest='surface_id', help='The surface id e.g. plywood, abs', default='plywood') parser.add_option('-r', '--real', action="store_true", dest='real_exp', help='Do the real experiment space', default=False) parser.add_option('', '--slow', action="store_true", dest='slow', help='Set slower global speed', default=False) (opt, args) = parser.parse_args() # set the parameters global globalvel global global_slow_vel globalvel = 300 # speed for moving around globalmaxacc = 100 # some big number means no limit, in m/s^2 globalacc = 1 # some big number means no limit, in m/s^2 global_slow_vel = 30 if opt.slow: globalvel = global_slow_vel ori = [0, 0, 1, 0] probe_id = 'probe4' probe_lengths = { 'probe1': 0.23746, 'probe2': 0.16649, 'probe3': 0.15947, 'probe4': 0.15653 } probe_length = probe_lengths[ probe_id] # probe1: 0.00626/2 probe2: 0.004745 probe3: 0.00475 ft_length = 0.04703 z = probe_length + ft_length + 0.004 + 0.00 # the height above the table # parameters about the surface surface_id = opt.surface_id surface_thicks = { 'plywood': 0.01158, 'abs': 0.01436, 'silicone_rubber': 0.01436 } surface_thick = surface_thicks[surface_id] # 0.01158 for plywood z = z + surface_thick z_recover = 0.012 + z # the height for recovery probe2: 0.2265 probe 3: 0.2226 zup = z + 0.08 + 0.1 # the prepare and end height probe_radii = { 'probe1': 0.00626 / 2, 'probe2': 0.004745, 'probe3': 0.00475 } probe_radius = probe_radii[probe_id] dist_before_contact = 0.03 dist_after_contact = 0.05 skip_when_exists = True reset_freq = 1 global_frame_id = '/map' # parameters about object shape_id = opt.shape_id shape_db = ShapeDB() shape_type = shape_db.shape_db[shape_id]['shape_type'] shape = shape_db.shape_db[shape_id]['shape'] obj_frame_id = shape_db.shape_db[shape_id]['frame_id'] obj_slot = shape_db.shape_db[shape_id]['slot_pos'] # space for the experiment real_exp = opt.real_exp if real_exp: #speeds = reversed([20, 50, 100, 200, 400]) speeds = reversed([-1, 20, 50, 100, 200, 400]) #speeds = reversed([20]) if shape_type == 'poly': side_params = np.linspace(0, 1, 11) else: side_params = np.linspace(0, 1, 40, endpoint=False) angles = np.linspace(-pi / 180.0 * 80.0, pi / 180 * 80, 9) else: speeds = [20, 100, 400, -1] #speeds = reversed([-1]) #shape = [shape[0]] side_params = [0.1] #np.linspace(0.1,0.9,3) angles = [0] #np.linspace(-pi/4, pi/4, 3) # parameters about rosbag dir_save_bagfile = os.environ[ 'PNPUSHDATA_BASE'] + '/straight_push/%s/push_dataset_motion_full_%s/' % ( surface_id, shape_id) #topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker'] topics = ['-a'] setAcc(acc=globalacc, deacc=globalacc) setSpeed(tcp=globalvel, ori=1000) setZone(0) make_sure_path_exists(dir_save_bagfile) # hack limit = 100 cnt = 0 # enumerate the speed for v in speeds: # enumerate the side we want to push for i in range(len(shape)): # enumerate the contact point that we want to push for s in side_params: if shape_type == 'poly': #pos = np.array(shape[i]) *s + np.array(shape[(i+1) % len(shape)]) *(1-s) pos = np.array(shape[i]) * (1 - s) + np.array(shape[ (i + 1) % len(shape)]) * (s) pos = np.append(pos, [0]) tangent = np.array(shape[(i + 1) % len(shape)]) - np.array( shape[i]) normal = np.array([tangent[1], -tangent[0]]) normal = normal / norm(normal) # normalize it normal = np.append(normal, [0]) elif shape_type == 'ellip': (a, b) = shape[0][0], shape[0][1] pos = [ shape[0][0] * np.cos(s * 2 * np.pi), shape[0][1] * np.sin(s * 2 * np.pi), 0 ] normal = [ np.cos(s * 2 * np.pi) / a, np.sin(s * 2 * np.pi) / b, 0 ] normal = normal / norm(normal) # normalize it elif shape_type == 'polyapprox': pos, normal = polyapprox(shape, s) # enumerate the direction in which we want to push for t in angles: bagfilename = 'motion_surface=%s_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % ( surface_id, shape_id, v, i, s, t) bagfilepath = dir_save_bagfile + bagfilename # if exists then skip it if skip_when_exists and os.path.isfile(bagfilepath): print bagfilepath, 'exits', 'skip' continue # find the probe pos in contact in object frame pos_probe_contact_object = pos + normal * probe_radius # find the start point direc = np.dot(tfm.euler_matrix(0, 0, t), normal.tolist() + [1])[0:3] # in the direction of moving out pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact if shape_type == 'polyapprox' and polyapprox_check_collision( shape, pos_start_probe_object, probe_radius): print bagfilename, 'will be in collision', 'skip' continue # find the end point pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact # zero force torque sensor rospy.sleep(0.1) setZero() wait_for_ft_calib() # transform start and end to world frame pos_start_probe_world = coordinateFrameTransform( pos_start_probe_object, obj_frame_id, global_frame_id, listener) pos_end_probe_world = coordinateFrameTransform( pos_end_probe_object, obj_frame_id, global_frame_id, listener) pos_contact_probe_world = coordinateFrameTransform( pos_probe_contact_object, obj_frame_id, global_frame_id, listener) # start bag recording # move to startPos start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = zup setCart(start_pos, ori) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = z setCart(start_pos, ori) rosbag_proc = subprocess.Popen( 'rosbag record -q -O %s %s' % (bagfilename, " ".join(topics)), shell=True, cwd=dir_save_bagfile) print 'rosbag_proc.pid=', rosbag_proc.pid rospy.sleep(0.5) end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = z if v > 0: # constant speed setAcc(acc=globalmaxacc, deacc=globalmaxacc) setSpeed(tcp=v, ori=1000) setCart(end_pos, ori) setSpeed(tcp=globalvel, ori=1000) setAcc(acc=globalacc, deacc=globalacc) else: # v < 0 acceleration setSpeed(tcp=30, ori=1000) # some slow speed mid_pos = copy.deepcopy(pos_contact_probe_world) mid_pos[2] = z setCart(mid_pos, ori) setAcc(acc=-v, deacc=globalmaxacc) setSpeed(tcp=1000, ori=1000) # some high speed setCart(end_pos, ori) setSpeed(tcp=globalvel, ori=1000) setAcc(acc=globalacc, deacc=globalacc) # end bag recording terminate_ros_node("/record") # move up vertically end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = zup setCart(end_pos, ori) # recover recover(obj_frame_id, global_frame_id, z_recover, obj_slot, not (cnt % reset_freq)) #pause() cnt += 1 if cnt > limit: break if cnt > limit: break if cnt > limit: break if cnt > limit: break
def push(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 = False): ## 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 ## initialize listener rospy rospy.init_node('listener', anonymous=True) listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) joint_topic = '/joint_states' # shelf variables pretensionDelta = 0.00 lipHeight = 0.035 ## 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 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 topple orientation (rotate wrist) toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] #[0, 0.7071, 0, 0.7071] # if gripper open close it closeGripper(forceThreshold) # 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 #pdb.set_trace() #TRACEEEEE fingerWidth = 0.06 offsetFinger = fingerWidth/2 targetHeight = (objPosition[2] - binFloorHeight)*2 effect = 'slide' if effect == 'topple': offsetEffect = targetHeight*3/4 elif effect == 'slide': offsetEffect = targetHeight*1/4 else: offsetEffect = targetHeight*1/4 #Default is slide, made three cases for flexibility offset = offsetFinger + offsetEffect targetPosition = [binMouth[0], objPosition[1], binFloorHeight+offset] 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() if s and effect == 'topple': print 'move to COM in y and above COM in z successful' elif s and effect == 'slide': print 'move to COM in y and below COM in z successful' else: print 'move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility if s: print 'tcp at:' print(targetPosition) plan.visualize() if isExecute: pauseFunc(withPause) plan.execute() else: print 'move to COM in y and above COM in z fail' qf = plan.q_traj[-1] # set second target, go over the lip of the bin #interiorLipBin = [0,0.39,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 = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic) #plan = planner.plan() #s = plan.success() #if s: # print 'move to inside the lip success' # print 'tcp at:' # print(targetPosition) # plan.visualize() # if isExecute: # pauseFunc(withPause) # plan.execute() #else: # print 'move to inside the lip fail' #qf = plan.q_traj[-1] ## close gripper fully (Do we wish to topple with gripper slightly open?) closeGripper(forceThreshold) ## 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 'stroke to end of bin success' print 'tcp at:' print(targetPosition) plan.visualize() if isExecute: pauseFunc(withPause) plan.execute() else: print 'stroke to end of bin fail' 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 'stroke to bin start success' print 'tcp at:' print(targetPosition) plan.visualize() if isExecute: pauseFunc(withPause) plan.execute() else: print 'stroke to bin start fail' 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 'return to mouth success' plan.visualize() if isExecute: pauseFunc(withPause) plan.execute() else: print 'return to mouth fail' return False
def push_back(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1], binNum=4, objId='crayola_64_ct', bin_contents=[ 'paper_mate_12_count_mirado_black_warrior', 'crayola_64_ct', 'expo_dry_erase_board_eraser' ], robotConfig=None, grasp_range_lim=0.11, fing_width=0.06, isExecute=True, withPause=True): #~ ***************************************************************** obj_dim = get_obj_dim(objId) obj_dim = np.array(obj_dim) #~ ***************************************************************** # # DEFINE HAND WIDTH######################### hand_width = 0.186 #~ ***************************************************************** ## initialize listener rospy listener = tf.TransformListener() rospy.sleep(0.1) br = tf.TransformBroadcaster() rospy.sleep(0.1) (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener) # print shelf_position, shelf_quaternion #~ ***************************************************************** # Convert xyx quat to tranformation matrix for Shelf frame #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7]) shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion) shelf_pose_tfm = np.array(shelf_pose_tfm_list) shelf_pose_orient = shelf_pose_tfm[0:3, 0:3] #~ print '[PushBack] shelf_pose_orient' #~ print shelf_pose_orient shelf_pose_pos = shelf_pose_tfm[0:3, 3] #~ print '[PushBack] shelf_pose_pos' #~ print shelf_pose_pos #Normalized axes of the shelf frame shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0]) shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1]) shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2]) #~ ***************************************************************** # Convert xyx quat to tranformaation matrix for Object frame 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] obj_pose_pos = obj_pose_tfm[0:3, 3] #~ print '[PushBack] obj_pose_orient' #~ print obj_pose_orient #Normalized axes of the object frame obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0]) #~ print '[PushBack] obj_X' #~ print obj_X obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1]) #~ print '[PushBack] obj_Y' #~ print obj_Y obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2]) #~ print '[PushBack] obj_Z' #~ print obj_Z #Normalized object frame obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z)) obj_pose_orient_norm = obj_pose_orient_norm.transpose() #~ print '[PushBack] obj_pose_orient_norm' #~ print obj_pose_orient_norm #~ ***************************************************************** # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin) # Find projection of object axes on Y axis of the shelf frame proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm) #~ print '[PushBack] proj' #~ print proj_vecY max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax( np.fabs(proj_vecY)) if proj_vecY[hand_norm_dir] > 0: hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir] else: hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir] #~ print '[PushBack] hand_norm_vec' #~ print hand_norm_vec #Find angle of the edge of the object Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) * la.norm(hand_norm_vec)) angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180 / np.pi #~ print'angle_to_shelfY' #~ print angle_to_shelfY #Find object dimension along hand normal axis obj_dim_along_hand_norm = obj_dim[hand_norm_dir] # print '[PushBack] dim along hand norm' # print obj_dim_along_hand_norm #~ ***************************************************************** #Find projection of object axes on X axis of the shelf frame #To find out which object frame is lying closer to the X axis of the shelf proj_vecX = np.dot(shelf_X, obj_pose_orient_norm) max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax( np.fabs(proj_vecX)) if proj_vecX[fing_axis_dir] > 0: fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir] else: fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir] #~ print '[PushBack] fing_axis_vec' #~ print fing_axis_vec #Find object dimension along the finger axis obj_dim_along_fingAxis = obj_dim[fing_axis_dir] # print '[PushBack] obj_dim_along_fingAxis' # print obj_dim_along_fingAxis #~ ***************************************************************** #Find projection of object axes on Z axis of the shelf frame #To find out which object frame is lying closer to the Z axis of the shelf proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm) max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax( np.fabs(proj_vecZ)) Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir] #~ print '[PushBack] Zaxis_vec' #~ print Zaxis_vec #Find object dimension along the finger axis, shelf Z obj_dim_along_ZAxis = obj_dim[Zaxis_dir] hand_Y = np.cross(hand_norm_vec, fing_axis_vec) diag_dist = la.norm( np.array([obj_dim_along_hand_norm, obj_dim_along_fingAxis])) # print'req obj diag dist=', diag_dist #~ ***************************************************************** # Find the maximum distance to which we will push other_diag = np.zeros(len(bin_contents)) for num_bin_contents in range(0, len(bin_contents)): if bin_contents[num_bin_contents] != objId: temp_obj_dim = get_obj_dim(objId) other_diag[num_bin_contents] = la.norm( np.array([temp_obj_dim[0], temp_obj_dim[1]])) max_diag, max_obj_ID = np.max(np.fabs(other_diag)), np.argmax( np.fabs(other_diag)) # print '[PushBack] max_obj_ID',max_diag # print '[PushBack] max_obj_ID', bin_contents[max_obj_ID] #******************************************************************* bin_inner_cnstr = get_bin_inner_cnstr() #******************************************************************* #Check if we can do left push diag_factor = 0.75 obj_left_edge = obj_pose_pos[1] + diag_factor * diag_dist / 2.0 binLeftWall = bin_inner_cnstr[binNum][1] binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf', 'map', listener) binRightWall = bin_inner_cnstr[binNum][0] binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0], 'shelf', 'map', listener) left_gap = binLeftWall_world.pose.position.y - obj_left_edge # print '[PushBack] left_gap=', left_gap fing_clearance = 0.010 # It's like 5 mm on both sides of finger bin_width = binLeftWall_world.pose.position.y - binRightWall_world.pose.position.y tolerance_in_push = 0.020 if left_gap + diag_dist / 2.0 - tolerance_in_push > bin_width: print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin' return (False, False) if left_gap > fing_width + fing_clearance: left_push = True print '[PushBack] left push is possible' else: left_push = False print '[PushBack] left push is NOT possible' # check if we can do right push obj_right_edge = obj_pose_pos[1] - diag_factor * diag_dist / 2.0 right_gap = obj_right_edge - binRightWall_world.pose.position.y # print '[PushBack] right_gap', right_gap fing_clearance = 0.010 # It's like 5 mm on both sides of finger if right_gap + diag_dist / 2.0 - tolerance_in_push > bin_width: print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin' return (False, False) if right_gap > fing_width + fing_clearance: right_push = True print '[PushBack] right push is possible' else: right_push = False print '[PushBack] right push is NOT possible' #******************************************************************* #Find Push Points for left push bin_face_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum) bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map', listener) tcp_Z_off = 0.005 #lower down from center of the bin push_tcp_Z_shelfFrame = ( (bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) / 2.0) - tcp_Z_off push_tcp_Z_WorldFrame = coordinateFrameTransform( [0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener) push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z push_side_off = 1.0 if left_push: left_pushPt_X = bin_face_pos_world.pose.position.x + 0.05 left_pushPt_Y = binLeftWall_world.pose.position.y - push_side_off * left_gap # left_Y_lim=binLeftWall_world.pose.position.y- left_pushPt_Z = deepcopy(push_tcp_Z) left_push_Pt = np.array([left_pushPt_X, left_pushPt_Y, left_pushPt_Z]) print '[PushBack] expected left push clearance=', binLeftWall_world.pose.position.y - left_pushPt_Y - fing_width / 2.0 #Find Push Points for right push if right_push: right_pushPt_X = bin_face_pos_world.pose.position.x + 0.05 right_pushPt_Y = binRightWall_world.pose.position.y + push_side_off * right_gap right_pushPt_Z = deepcopy(push_tcp_Z) right_push_Pt = np.array( [right_pushPt_X, right_pushPt_Y, right_pushPt_Z]) print '[PushBack] expected right push clearance=', right_pushPt_Y - binRightWall_world.pose.position.y - fing_width / 2.0 #Execute push trajectories #~ ************************************************************* # set spatual down orientation (rotate wrist) spatula_down_orient = [0, 0.7071, 0, 0.7071] push_orient = spatula_down_orient push_hand_opening = 0.100 joint_topic = '/joint_states' #~ ************************************************************* #Execure left push trajectory if left_push: # plan store left_plans = [] # set tcp (same as that set in generate dictionary) l1 = 0.0 l2 = 0.0 l3 = 0.47 tip_hand_transform = [ l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [ mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z ] gripperOri = [0, 0.7071, 0, 0.7071] pubFrame(br, pose=targetPosition + gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushBack] move to bin mouth in push-back code successful (left push)' left_plans.append(plan) # Plan 0 else: print '[PushBack] move to bin mouth in push-back code fail (left push)' return (False, False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) left_plans.append(grasp_plan) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1 = 0.0 push_l2 = 0.0 push_l3 = 0.47 push_tip_hand_transform = [ l1, l2, l3, 0, 0, 0 ] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) distFromShelf = -0.015 InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [ InbinPt.pose.position.x, left_push_Pt[1], InbinPt.pose.position.z ] #matching world_Y of first push pt pubFrame(br, pose=prepush_targetPosition + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0=q_initial, target_tip_pos=prepush_targetPosition, target_tip_ori=push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] move inside bin in push orient successful' #~ print '[PushBack] tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() left_plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushBack] move inside bin in push orient fail' return (False, False) qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH############### # Open the gripper to dim calculated for pushing print '[PushBack] hand opening to' print push_hand_opening * 1000.0 grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) left_plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## back_bin_X = bin_face_pos_world.pose.position.x + 0.43 push_stop_worldX = back_bin_X - max_diag - 0.010 left_pushStop_Pt_pos = deepcopy(left_push_Pt) left_pushStop_Pt_pos[0] = push_stop_worldX #Push until the end planner = IK(q0=q_initial, target_tip_pos=left_pushStop_Pt_pos, target_tip_ori=push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] start point push traj successful' left_plans.append(plan) # Plan 1 else: print '[PushBack] start point push traj fail' return (False, False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) left_plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(left_plans)): if isExecute: left_plans[numOfPlan].visualize() pauseFunc(withPause) left_plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(left_plans)): if isExecute: left_plans[numOfPlan].visualizeBackward() pauseFunc(withPause) left_plans[len(left_plans) - numOfPlan - 1].executeBackward() #******************************************************************* if right_push: # plan store right_plans = [] # set tcp (same as that set in generate dictionary) l1 = 0.0 l2 = 0.0 l3 = 0.47 tip_hand_transform = [ l1, l2, l3, 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) #~ ************************************************************* # GO TO MOUTH OF THE BIN q_initial = robotConfig # move to bin mouth distFromShelf = 0.1 mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum) mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener) targetPosition = [ mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z ] gripperOri = [0, 0.7071, 0, 0.7071] pubFrame(br, pose=targetPosition + gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5) 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() plan.setSpeedByName('faster') s = plan.success() if s: print '[PushBack] move to bin mouth in push-back code successful (right push)' right_plans.append(plan) # Plan 0 else: print '[PushBack] move to bin mouth in push-back code fail (right push)' return (False, False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) right_plans.append(grasp_plan) #~ ************************************************************* ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ###### # set push_tcp push_l1 = 0.0 push_l2 = 0.0 push_l3 = 0.47 push_tip_hand_transform = [ l1, l2, l3, 0, 0, 0 ] # to be updated when we have a hand design finalized # broadcast frame attached to grasp_tcp pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10) distFromShelf = -0.015 InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum) InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener) prepush_targetPosition = [ InbinPt.pose.position.x, right_push_Pt[1], InbinPt.pose.position.z ] #matching world_Y of first push pt pubFrame(br, pose=prepush_targetPosition + push_orient, frame_id='target_pose', parent_frame_id='map', npub=10) planner = IK(q0=q_initial, target_tip_pos=prepush_targetPosition, target_tip_ori=push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic) plan = planner.plan() s = plan.success() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] move inside bin in push orient successful' #~ print '[PushBack] tcp at:' #~ print(pregrasp_targetPosition) #~ plan.visualize() right_plans.append(plan) # Plan 1 #~ if isExecute: #~ pauseFunc(withPause) #~ plan.execute() else: print '[PushBack] move inside bin in push orient fail' return (False, False) qf = plan.q_traj[-1] q_initial = qf #~ ************************************************************* ############## OPEN THE GRIPPER To PUSH############### # Open the gripper to dim calculated for pushing print '[PushBack] hand opening to' print push_hand_opening * 1000.0 grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening)) right_plans.append(grasp_plan) #~ ************************************************************* ############ Execute the push trajectory ############## back_bin_X = bin_face_pos_world.pose.position.x + 0.42 push_stop_worldX = back_bin_X - max_diag - 0.010 right_pushStop_Pt_pos = deepcopy(right_push_Pt) right_pushStop_Pt_pos[0] = push_stop_worldX #Push until the end planner = IK(q0=q_initial, target_tip_pos=right_pushStop_Pt_pos, target_tip_ori=push_orient, joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform) plan = planner.plan() plan.setSpeedByName('fast') s = plan.success() if s: print '[PushBack] start point push traj successful' right_plans.append(plan) # Plan 1 else: print '[PushBack] start point push traj fail' return (False, False) qf = plan.q_traj[-1] q_initial = qf #################### CLOSE THE GRIPPER GRASP ################# grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0)) right_plans.append(grasp_plan) #~ ************************************************************* # #################### EXECUTE FORWARD #################### for numOfPlan in range(0, len(right_plans)): if isExecute: right_plans[numOfPlan].visualize() pauseFunc(withPause) right_plans[numOfPlan].execute() # #################### RETREAT #################### for numOfPlan in range(0, len(right_plans)): if isExecute: right_plans[numOfPlan].visualizeBackward() pauseFunc(withPause) right_plans[len(right_plans) - numOfPlan - 1].executeBackward() if right_push or left_push: push_possible = True else: push_possible = False return (push_possible, False)
def main(argv): # prepare the proxy, listener global listener global vizpub rospy.init_node('collect_motion_data') listener = tf.TransformListener() vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10) br = TransformBroadcaster() parser = optparse.OptionParser() parser.add_option('-s', action="store", dest='shape_id', help='The shape id e.g. rect1-rect3', default='rect1') parser.add_option('', '--surface', action="store", dest='surface_id', help='The surface id e.g. plywood, abs', default='plywood') parser.add_option('-r', '--real', action="store_true", dest='real_exp', help='Do the real experiment space', default=False) parser.add_option('', '--slow', action="store_true", dest='slow', help='Set slower global speed', default=False) (opt, args) = parser.parse_args() # set the parameters global globalvel global global_slow_vel globalvel = 300 # speed for moving around globalmaxacc = 100 # some big number means no limit, in m/s^2 globalacc = 1 # some big number means no limit, in m/s^2 global_slow_vel = 30 if opt.slow: globalvel = global_slow_vel ori = [0, 0, 1, 0] probe_id = 'probe4' probe_lengths = {'probe1' : 0.23746, 'probe2': 0.16649, 'probe3': 0.15947, 'probe4': 0.15653} probe_length = probe_lengths[probe_id] # probe1: 0.00626/2 probe2: 0.004745 probe3: 0.00475 ft_length = 0.04703 z = probe_length + ft_length + 0.004 + 0.00 # the height above the table # parameters about the surface surface_id = opt.surface_id surface_thicks = {'plywood': 0.01158, 'abs': 0.01436, 'silicone_rubber': 0.01436} surface_thick = surface_thicks[surface_id] # 0.01158 for plywood z = z + surface_thick z_recover = 0.012 + z # the height for recovery probe2: 0.2265 probe 3: 0.2226 zup = z + 0.08 +0.1 # the prepare and end height probe_radii = {'probe1' : 0.00626/2, 'probe2': 0.004745, 'probe3': 0.00475} probe_radius = probe_radii[probe_id] dist_before_contact = 0.03 dist_after_contact = 0.05 skip_when_exists = True reset_freq = 1 global_frame_id = '/map' # parameters about object shape_id = opt.shape_id shape_db = ShapeDB() shape_type = shape_db.shape_db[shape_id]['shape_type'] shape = shape_db.shape_db[shape_id]['shape'] obj_frame_id = shape_db.shape_db[shape_id]['frame_id'] obj_slot = shape_db.shape_db[shape_id]['slot_pos'] # space for the experiment real_exp = opt.real_exp if real_exp: #speeds = reversed([20, 50, 100, 200, 400]) speeds = reversed([-1, 20, 50, 100, 200, 400]) #speeds = reversed([20]) if shape_type == 'poly': side_params = np.linspace(0, 1, 11) else: side_params = np.linspace(0,1,40,endpoint=False) angles = np.linspace(-pi/180.0*80.0, pi/180*80, 9) else: speeds = [20, 100, 400, -1] #speeds = reversed([-1]) #shape = [shape[0]] side_params = [0.1]#np.linspace(0.1,0.9,3) angles = [0] #np.linspace(-pi/4, pi/4, 3) # parameters about rosbag dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/straight_push/%s/push_dataset_motion_full_%s/' % (surface_id,shape_id) #topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker'] topics = ['-a'] setAcc(acc=globalacc, deacc=globalacc) setSpeed(tcp=globalvel, ori=1000) setZone(0) make_sure_path_exists(dir_save_bagfile) # hack limit = 100 cnt = 0 # enumerate the speed for v in speeds: # enumerate the side we want to push for i in range(len(shape)): # enumerate the contact point that we want to push for s in side_params: if shape_type == 'poly': #pos = np.array(shape[i]) *s + np.array(shape[(i+1) % len(shape)]) *(1-s) pos = np.array(shape[i]) *(1-s) + np.array(shape[(i+1) % len(shape)]) *(s) pos = np.append(pos, [0]) tangent = np.array(shape[(i+1) % len(shape)]) - np.array(shape[i]) normal = np.array([tangent[1], -tangent[0]]) normal = normal / norm(normal) # normalize it normal = np.append(normal, [0]) elif shape_type == 'ellip': (a,b) = shape[0][0], shape[0][1] pos = [shape[0][0] * np.cos(s*2*np.pi), shape[0][1] * np.sin(s*2*np.pi), 0] normal = [np.cos(s*2*np.pi)/a, np.sin(s*2*np.pi)/b, 0] normal = normal / norm(normal) # normalize it elif shape_type == 'polyapprox': pos, normal = polyapprox(shape, s) # enumerate the direction in which we want to push for t in angles: bagfilename = 'motion_surface=%s_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (surface_id, shape_id, v, i, s, t) bagfilepath = dir_save_bagfile+bagfilename # if exists then skip it if skip_when_exists and os.path.isfile(bagfilepath): print bagfilepath, 'exits', 'skip' continue # find the probe pos in contact in object frame pos_probe_contact_object = pos + normal * probe_radius # find the start point direc = np.dot(tfm.euler_matrix(0,0,t) , normal.tolist() + [1])[0:3] # in the direction of moving out pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact if shape_type == 'polyapprox' and polyapprox_check_collision(shape, pos_start_probe_object, probe_radius): print bagfilename, 'will be in collision', 'skip' continue # find the end point pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact # zero force torque sensor rospy.sleep(0.1) setZero() wait_for_ft_calib() # transform start and end to world frame pos_start_probe_world = coordinateFrameTransform(pos_start_probe_object, obj_frame_id, global_frame_id, listener) pos_end_probe_world = coordinateFrameTransform(pos_end_probe_object, obj_frame_id, global_frame_id, listener) pos_contact_probe_world = coordinateFrameTransform(pos_probe_contact_object, obj_frame_id, global_frame_id, listener) # start bag recording # move to startPos start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = zup setCart(start_pos,ori) start_pos = copy.deepcopy(pos_start_probe_world) start_pos[2] = z setCart(start_pos,ori) rosbag_proc = subprocess.Popen('rosbag record -q -O %s %s' % (bagfilename, " ".join(topics)) , shell=True, cwd=dir_save_bagfile) print 'rosbag_proc.pid=', rosbag_proc.pid rospy.sleep(0.5) end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = z if v > 0: # constant speed setAcc(acc=globalmaxacc, deacc=globalmaxacc) setSpeed(tcp=v, ori=1000) setCart(end_pos,ori) setSpeed(tcp=globalvel, ori=1000) setAcc(acc=globalacc, deacc=globalacc) else: # v < 0 acceleration setSpeed(tcp=30, ori=1000) # some slow speed mid_pos = copy.deepcopy(pos_contact_probe_world) mid_pos[2] = z setCart(mid_pos,ori) setAcc(acc=-v, deacc=globalmaxacc) setSpeed(tcp=1000, ori=1000) # some high speed setCart(end_pos,ori) setSpeed(tcp=globalvel, ori=1000) setAcc(acc=globalacc, deacc=globalacc) # end bag recording terminate_ros_node("/record") # move up vertically end_pos = copy.deepcopy(pos_end_probe_world) end_pos[2] = zup setCart(end_pos,ori) # recover recover(obj_frame_id, global_frame_id, z_recover, obj_slot, not(cnt % reset_freq)) #pause() cnt += 1 if cnt > limit: break; if cnt > limit: break; if cnt > limit: break; if cnt > limit: break;