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(
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'
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()