def goToHomeSuction(plan=None): joint_topic = '/joint_states' ## initialize listener rospy listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.1) while True: APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() q0 = APCrobotjoints.position if len(q0) < 6: continue else: break plan_n = Plan() if q0[5] < 0: possible_start_config = [ 0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539, 0.09102420595, 3.33888627518 - 2 * math.pi ] else: possible_start_config = [ 0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539, 0.09102420595, 3.33888627518 ] plan_n.q_traj = [possible_start_config] return plan_n
import math import numpy as np import tf.transformations as tfm import capsen.capsen from ik.roshelper import pubFrame # bin_1 # 1.1069; -0.018219; 1.3762 -0.54475; 0.075625; 0.20345; 0.81003 #-0.035814156250925766, 0.5470560007467684, -0.8243085337401062, -2.70526034059, 0.8721846604532582, -0.3148573970598978 rospy.init_node('listener', anonymous=True) #desired_joint_pose = [-0.0087, 0.6423, -0.1222, 1.4731, 1.4643, 1.2] # for bin 7 home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4] plan = Plan() plan.q_traj = [home_joint_pose] plan.visualize() plan.execute() br = tf.TransformBroadcaster() # generate targets for horizontal scan #bin_cnstr = get_bin_cnstr() bin_cnstr = get_bin_inner_cnstr() target_poses = [] refind = 7 for i in range(12): #target_pose_7_vert = [1.1756, 0, 0.79966, 0.69537, 0.11132, 0.049168, 0.70827] # for bin 7 vertical #target_pose_7_hori = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743] # for bin 7 horizontal target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler( math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
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 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 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)
import tf import math import numpy as np import tf.transformations as tfm import capsen.capsen from ik.roshelper import pubFrame # bin_1 # 1.1069; -0.018219; 1.3762 -0.54475; 0.075625; 0.20345; 0.81003 #-0.035814156250925766, 0.5470560007467684, -0.8243085337401062, -2.70526034059, 0.8721846604532582, -0.3148573970598978 rospy.init_node('listener', anonymous=True) #desired_joint_pose = [-0.0087, 0.6423, -0.1222, 1.4731, 1.4643, 1.2] # for bin 7 home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4] plan = Plan(); plan.q_traj = [home_joint_pose]; plan.visualize(); plan.execute() br = tf.TransformBroadcaster() # generate targets for horizontal scan #bin_cnstr = get_bin_cnstr() bin_cnstr = get_bin_inner_cnstr() target_poses = [] refind = 7 for i in range(12): #target_pose_7_vert = [1.1756, 0, 0.79966, 0.69537, 0.11132, 0.049168, 0.70827] # for bin 7 vertical #target_pose_7_hori = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743] # for bin 7 horizontal target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist() target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist()