Ejemplo n.º 1
0
def make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0):
    "do ik and then fill in the points where ik failed"

    n = len(xyzs)
    assert len(quats) == n
    
    robot = manip.GetRobot()
    joint_inds = manip.GetArmIndices()
    robot.SetActiveDOFs(joint_inds)
    orig_joint = robot.GetActiveDOFValues()
    
    joints = []
    inds = []

    for i in xrange(0,n):
        mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
        joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
        if joint is not None: 
            joints.append(joint)
            inds.append(i)
            robot.SetActiveDOFValues(joint)


    robot.SetActiveDOFValues(orig_joint)
    
    
    rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
    if len(inds) > 2:
        joints2 = mu.interp2d(np.arange(n), inds, joints)
        return joints2, inds
    else:
        return np.zeros((n, len(joints))), []
Ejemplo n.º 2
0
def make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame,
                    filter_options):
    """
    do ik to make a joint trajectory
    joint_seeds are the joint angles that this function will try to be close to
    I put that in so you could try to stay near the demonstration joint angles
    in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
    (TODO)
    """
    n = len(xyzs)
    assert len(quats) == n

    robot = manip.GetRobot()
    robot.SetActiveManipulator(manip.GetName())
    joint_inds = manip.GetArmIndices()
    orig_joint = robot.GetDOFValues(joint_inds)

    joints = []
    inds = []

    for i in xrange(0, n):
        mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
        robot.SetDOFValues(joint_seeds[i], joint_inds)
        joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
                                  filter_options)
        if joint is not None:
            joints.append(joint)
            inds.append(i)
            robot.SetDOFValues(joint, joint_inds)

    robot.SetDOFValues(orig_joint, joint_inds)

    rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
    if len(inds) > 2:
        joints2 = mu.interp2d(np.arange(n), inds, joints)
        return joints2, inds
    else:
        return np.zeros((n, len(joints))), []
Ejemplo n.º 3
0
def make_joint_traj(xyzs, quats, joint_seeds,manip, ref_frame, targ_frame,filter_options):
    """
    do ik to make a joint trajectory
    joint_seeds are the joint angles that this function will try to be close to
    I put that in so you could try to stay near the demonstration joint angles
    in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
    (TODO)
    """
    n = len(xyzs)
    assert len(quats) == n
    
    robot = manip.GetRobot()
    robot.SetActiveManipulator(manip.GetName())
    joint_inds = manip.GetArmIndices()
    orig_joint = robot.GetDOFValues(joint_inds)

    joints = []
    inds = []

    for i in xrange(0,n):
        mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
        robot.SetDOFValues(joint_seeds[i], joint_inds)
        joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
        if joint is not None: 
            joints.append(joint)
            inds.append(i)
            robot.SetDOFValues(joint, joint_inds)

    robot.SetDOFValues(orig_joint, joint_inds)

    rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
    if len(inds) > 2:
        joints2 = mu.interp2d(np.arange(n), inds, joints)
        return joints2, inds
    else:
        return np.zeros((n, len(joints))), []
Ejemplo n.º 4
0
def make_joint_traj(xyzs,
                    quats,
                    manip,
                    ref_frame,
                    targ_frame,
                    filter_options=0):
    "do ik and then fill in the points where ik failed"

    n = len(xyzs)
    assert len(quats) == n

    robot = manip.GetRobot()
    joint_inds = manip.GetArmIndices()
    robot.SetActiveDOFs(joint_inds)
    orig_joint = robot.GetActiveDOFValues()

    joints = []
    inds = []

    for i in xrange(0, n):
        mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
        joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
                                  filter_options)
        if joint is not None:
            joints.append(joint)
            inds.append(i)
            robot.SetActiveDOFValues(joint)

    robot.SetActiveDOFValues(orig_joint)

    rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
    if len(inds) > 2:
        joints2 = mu.interp2d(np.arange(n), inds, joints)
        return joints2, inds
    else:
        return np.zeros((n, len(joints))), []
Ejemplo n.º 5
0
        pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
        pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id)                
        
        n_sel = F[verb]["nargs"].value    
        arg_clouds = []
        for i_sel in xrange(n_sel):
            pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
            xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
            arg_clouds.append(xyz.reshape(-1,3))
            
        make_and_execute_verb_traj(verb, arg_clouds)   
        raw_input("press enter to continue")        
                                
    elif verb in ["swap-tool"]:
        
        l_switch_posture = np.array([  0.773,   0.595,   1.563,  -1.433,   0.478,  -0.862,  .864])
        
        lr = select_from_list(["left", "right"])
        if lr == 'l':
            arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
        else:
            arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(l_switch_posture)
            
        arm.goto_joint_positions(jpos)
        grip.open()
        raw_input("press enter when ready")
        grip.close()
        pr2.join_all()
                
        
Ejemplo n.º 6
0
                                               pc.header.frame_id)

        n_sel = F[verb]["nargs"].value
        arg_clouds = []
        for i_sel in xrange(n_sel):
            pc_sel = seg_svc.call(
                ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
            xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
            arg_clouds.append(xyz.reshape(-1, 3))

        make_and_execute_verb_traj(verb, arg_clouds)
        raw_input("press enter to continue")

    elif verb in ["swap-tool"]:

        l_switch_posture = np.array(
            [0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864])

        lr = select_from_list(["left", "right"])
        if lr == 'l':
            arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
        else:
            arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(
                l_switch_posture)

        arm.goto_joint_positions(jpos)
        grip.open()
        raw_input("press enter when ready")
        grip.close()
        pr2.join_all()
Ejemplo n.º 7
0
import utils.conversions as conv
from brett2.PR2 import *
#from brett2.PR2 import mirror_arm_joints
from brett2.ghost_PR2 import GhostPR2

from math import pi
import tf
import numpy
 

action_client = []

if __name__=='__main__':
    rospy.init_node('ankush')
    bot = PR2()
    rospy.loginfo(bot.base.get_pose())

    #bot.base.goto_pose(10,5.0,pi/2, '/map')
    #bot.torso.go_up()
    p = numpy.array([1,0,0])
    bot.head.look_at(p, 'base_link', 'narrow_stereo_link')


    fold_reset = [pi/32,  pi/12,   0, -pi/2,  pi ,  -pi/6,  -pi/4]    
    bot.larm.goto_joint_positions(fold_reset)
    bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))

    rospy.sleep(4)

    bot.lgrip.open()
Ejemplo n.º 8
0
    parser.add_argument("arms_used")
    parser.add_argument("--dry_run", action="store_true")
    args = parser.parse_args()
    return args

if __name__ == "__main__":
    if rospy.get_name() == "/unnamed": 
        rospy.init_node("multi_item_teach_verb", disable_signals=True)

    args = get_args()
    # arguments need to be: <verb> <items separated by ','> <arms_used separated by ',' and corresponding to items>
    verb = args.verb
    items_str = args.items
    items = items_str.split(",")
    arms_used = args.arms_used.split(",")
    data_dir = osp.join(osp.dirname(lfd.__file__), "data")

    pr2 = PR2.PR2()
    move_pr2_to_start_pos(pr2)

    demo_name = get_new_demo_name(verb, items)
    stage_names_for_demo, special_pts = do_teach_verb(demo_name, items, arms_used, data_dir)
    new_entry_text = get_new_demo_entry_text(demo_name, stage_names_for_demo, items, arms_used, special_pts)

    yn = yes_or_no("save demonstration?")
    # add the entry to the verbs yaml file
    if yn:
        add_new_entry_to_yaml_file(data_dir, new_entry_text)
    else:
        print "exiting without saving"