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(
Exemple #2
0
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()