예제 #1
0
def fit_and_plot_n(n=100, regrot=True, draw_plinks=True, fine=False, augment_coords=False):
    """
    function for timing.
    """
    sc = [np.random.randn(n,3)/100.]
    tc = [np.random.randn(n,3)/100.]
    
    start = time.time()
    if regrot:
        test_tps_rpm_regrot_multi(sc, tc, fine=fine, augment_coords=augment_coords)
    else:
        registration.tps_rpm(sc[0], tc[0])
    end = time.time()
    print colorize("It took : %f seconds."%(end - start), "red", True)
예제 #2
0
def find_closest_human(demofile, new_xyz):
    "for now, just prompt the user"
    seg_names = demofile.keys()
    print "choose from the following options (type an integer)"
    for (i, seg_name) in enumerate(seg_names):
        print "%i: %s" % (i, seg_name)
    while True:
        try:
            choice_ind = int(raw_input())
            chosen_seg = seg_names[choice_ind]
            seg_info = demofile[chosen_seg]
            old_xyz = np.squeeze(seg_info["cloud_xyz"])
            handles = []
            handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1)))
            handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1)))

            f = registration.tps_rpm(old_xyz, new_xyz, plot_cb=tpsrpm_plot_cb, plotting=1)
            handles.extend(
                plotting_openrave.draw_grid(
                    Globals.env,
                    f.transform_points,
                    old_xyz.min(axis=0),
                    old_xyz.max(axis=0),
                    xres=0.1,
                    yres=0.1,
                    zres=0.04,
                )
            )
            inv_f = registration.tps_rpm(new_xyz, old_xyz)
            K = -ssd.squareform(ssd.pdist(f.x_na))
            w = f.w_ng
            inv_tps_cost = np.trace(w.T.dot(K).dot(w))
            K = -ssd.squareform(ssd.pdist(inv_f.x_na))
            w = inv_f.w_ng
            tps_cost = np.trace(w.T.dot(K).dot(w))

            print "Inverse TPS Cost: %f" % (inv_tps_cost)
            print "TPS Cost: %f" % (tps_cost)
            response = raw_input()
            break
        except (IndexError, ValueError):
            print "invalid selection. try again"
        # print "confirm selection (y/n)"
        # response = raw_input()
        # if response == "y":
        #    break
        # print "choose from the following options (type an integer)"
        # for (i, seg_name) in enumerate(seg_names):
        #    print "%i: %s"%(i,seg_name)
    return f, chosen_seg
예제 #3
0
def find_closest_human(demofile, new_xyz):
    "for now, just prompt the user"
    seg_names = demofile.keys()
    print "choose from the following options (type an integer)"
    for (i, seg_name) in enumerate(seg_names):
        print "%i: %s" % (i, seg_name)
    while True:
        try:
            choice_ind = int(raw_input())
            chosen_seg = seg_names[choice_ind]
            seg_info = demofile[chosen_seg]
            old_xyz = np.squeeze(seg_info["cloud_xyz"])
            handles = []
            handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1)))
            handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1)))

            f = registration.tps_rpm(old_xyz,
                                     new_xyz,
                                     plot_cb=tpsrpm_plot_cb,
                                     plotting=1)
            handles.extend(
                plotting_openrave.draw_grid(Globals.env,
                                            f.transform_points,
                                            old_xyz.min(axis=0),
                                            old_xyz.max(axis=0),
                                            xres=.1,
                                            yres=.1,
                                            zres=.04))
            inv_f = registration.tps_rpm(new_xyz, old_xyz)
            K = -ssd.squareform(ssd.pdist(f.x_na))
            w = f.w_ng
            inv_tps_cost = np.trace(w.T.dot(K).dot(w))
            K = -ssd.squareform(ssd.pdist(inv_f.x_na))
            w = inv_f.w_ng
            tps_cost = np.trace(w.T.dot(K).dot(w))

            print "Inverse TPS Cost: %f" % (inv_tps_cost)
            print "TPS Cost: %f" % (tps_cost)
            response = raw_input()
            break
        except (IndexError, ValueError):
            print "invalid selection. try again"
        #print "confirm selection (y/n)"
        #response = raw_input()
        #if response == "y":
        #    break
        #print "choose from the following options (type an integer)"
        #for (i, seg_name) in enumerate(seg_names):
        #    print "%i: %s"%(i,seg_name)
    return f, chosen_seg
예제 #4
0
def fit_and_plot_n(n=100,
                   regrot=True,
                   draw_plinks=True,
                   fine=False,
                   augment_coords=False):
    """
    function for timing.
    """
    sc = [np.random.randn(n, 3) / 100.]
    tc = [np.random.randn(n, 3) / 100.]

    start = time.time()
    if regrot:
        test_tps_rpm_regrot_multi(sc,
                                  tc,
                                  fine=fine,
                                  augment_coords=augment_coords)
    else:
        registration.tps_rpm(sc[0], tc[0])
    end = time.time()
    print colorize("It took : %f seconds." % (end - start), "red", True)
예제 #5
0
def main():

    demofile = h5py.File(args.h5file, 'r')
    
    trajoptpy.SetInteractive(args.interactive)

    if args.execution:
        rospy.init_node("exec_task",disable_signals=True)
        Globals.pr2 = PR2.PR2()
        Globals.env = Globals.pr2.env
        Globals.robot = Globals.pr2.robot
        
    else:
        Globals.env = openravepy.Environment()
        Globals.env.StopSimulation()
        Globals.env.Load("robots/pr2-beta-static.zae")    
        Globals.robot = Globals.env.GetRobots()[0]

    if not args.fake_data_segment:
        grabber = cloudprocpy.CloudGrabber()
        grabber.startRGBD()

    Globals.viewer = trajoptpy.GetViewer(Globals.env)

    #####################

    while True:
        
    
        redprint("Acquire point cloud")
        if args.fake_data_segment:
            new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"])
            hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6])
            hmat[:3,3] = args.fake_data_transform[0:3]
            new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:]
            
        else:
            
            Globals.pr2.rarm.goto_posture('side')
            Globals.pr2.larm.goto_posture('side')            
            Globals.pr2.join_all()
            
            Globals.pr2.update_rave()            
            
            rgb, depth = grabber.getRGBD()
            T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
            new_xyz = cloud_proc_func(rgb, depth, T_w_k)

    
        ################################    
        redprint("Finding closest demonstration")
        if args.fake_data_segment:
            seg_name = args.fake_data_segment
        else:
            seg_name = find_closest(demofile, new_xyz)
        
        seg_info = demofile[seg_name]
        # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))
    
        ################################

        redprint("Generating end-effector trajectory")    

        handles = []
        old_xyz = np.squeeze(seg_info["cloud_xyz"])
        handles.append(Globals.env.plot3(old_xyz,5, (1,0,0,1)))
        handles.append(Globals.env.plot3(new_xyz,5, (0,0,1,1)))


        f = registration.tps_rpm(old_xyz, new_xyz, plot_cb = tpsrpm_plot_cb,plotting=1)            
        
        #f = registration.ThinPlateSpline() XXX XXX
        
        handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0), old_xyz.max(axis=0), xres = .1, yres = .1, zres = .04))
        

        link2eetraj = {}
        for lr in 'lr':
            link_name = "%s_gripper_tool_frame"%lr
            old_ee_traj = asarray(seg_info[link_name]["hmat"])
            new_ee_traj = f.transform_hmats(old_ee_traj)
            link2eetraj[link_name] = new_ee_traj
            
            handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
            handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
            
            
            
        # TODO plot
        # plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj)
    
        miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)    
        success = True
        print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
        for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
            
            if args.execution=="real": Globals.pr2.update_rave()


            ################################    
            redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))

            bodypart2traj = {}
            
            arms_used = ""
        
            for lr in 'lr':
                manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
                old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1])
                if arm_moved(old_joint_traj):          
                    ee_link_name = "%s_gripper_tool_frame"%lr
                    new_ee_traj = link2eetraj[ee_link_name]
                    if args.execution: Globals.pr2.update_rave()                    
                    new_joint_traj = plan_follow_traj(Globals.robot, manip_name,
                     Globals.robot.GetLink(ee_link_name), new_ee_traj[i_start:i_end+1], 
                     old_joint_traj)
                    # (robot, manip_name, ee_link, new_hmats, old_traj):
                    part_name = {"l":"larm", "r":"rarm"}[lr]
                    bodypart2traj[part_name] = new_joint_traj
                    arms_used += lr

        

            ################################    
            redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, arms_used))

            for lr in 'lr':
                set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
            #trajoptpy.GetViewer(Globals.env).Idle()
        
            if len(bodypart2traj) > 0:
                exec_traj_maybesim(bodypart2traj)
        
            # TODO measure failure condtions

            if not success:
                break
            
        redprint("Segment %s result: %s"%(seg_name, success))
    
    
        if args.fake_data_segment: break
예제 #6
0
def main():

    demofile = h5py.File(args.h5file, 'r')

    trajoptpy.SetInteractive(args.interactive)

    if args.execution:
        rospy.init_node("exec_task", disable_signals=True)
        Globals.pr2 = PR2.PR2()
        Globals.env = Globals.pr2.env
        Globals.robot = Globals.pr2.robot

    else:
        Globals.env = openravepy.Environment()
        Globals.env.StopSimulation()
        Globals.env.Load("robots/pr2-beta-static.zae")
        Globals.robot = Globals.env.GetRobots()[0]

    if not args.fake_data_segment:
        subprocess.call("killall XnSensorServer", shell=True)
        grabber = cloudprocpy.CloudGrabber()
        grabber.startRGBD()

    Globals.viewer = trajoptpy.GetViewer(Globals.env)

    #####################

    while True:

        redprint("Acquire point cloud")
        if args.fake_data_segment:
            new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"])
            hmat = openravepy.matrixFromAxisAngle(
                args.fake_data_transform[3:6])
            hmat[:3, 3] = args.fake_data_transform[0:3]
            new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]

        else:

            Globals.pr2.rarm.goto_posture('side')
            Globals.pr2.larm.goto_posture('side')
            Globals.pr2.join_all()

            Globals.pr2.update_rave()

            rgb, depth = grabber.getRGBD()
            T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
            new_xyz = cloud_proc_func(rgb, depth, T_w_k)

        ################################
        redprint("Finding closest demonstration")
        if args.fake_data_segment:
            seg_name = args.fake_data_segment
        elif args.choice == "costs":
            f, seg_name = choose_segment(demofile, new_xyz)
        else:
            seg_name = choose_segment(demofile, new_xyz)

        seg_info = demofile[seg_name]
        # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))

        ################################

        redprint("Generating end-effector trajectory")
        if not args.choice == "costs":
            handles = []
            old_xyz = np.squeeze(seg_info["cloud_xyz"])
            handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1)))
            handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1)))
            f = registration.tps_rpm(old_xyz,
                                     new_xyz,
                                     plot_cb=tpsrpm_plot_cb,
                                     plotting=1)

            handles.extend(
                plotting_openrave.draw_grid(Globals.env,
                                            f.transform_points,
                                            old_xyz.min(axis=0),
                                            old_xyz.max(axis=0),
                                            xres=.1,
                                            yres=.1,
                                            zres=.04))

        link2eetraj = {}
        for lr in 'lr':
            link_name = "%s_gripper_tool_frame" % lr
            old_ee_traj = asarray(seg_info[link_name]["hmat"])
            new_ee_traj = f.transform_hmats(old_ee_traj)
            link2eetraj[link_name] = new_ee_traj
            if not args.choice == "costs":
                handles.append(
                    Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
                                              (1, 0, 0, 1)))
                handles.append(
                    Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
                                              (0, 1, 0, 1)))

        # TODO plot
        # plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj)

        miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
        success = True
        print colorize.colorize("mini segments:",
                                "red"), miniseg_starts, miniseg_ends
        for (i_miniseg,
             (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):

            if args.execution == "real": Globals.pr2.update_rave()

            ################################
            redprint("Generating joint trajectory for segment %s, part %i" %
                     (seg_name, i_miniseg))

            bodypart2traj = {}

            arms_used = ""

            for lr in 'lr':
                manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
                old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
                                                              1])
                if arm_moved(old_joint_traj):
                    ee_link_name = "%s_gripper_tool_frame" % lr
                    new_ee_traj = link2eetraj[ee_link_name]
                    if args.execution: Globals.pr2.update_rave()
                    new_joint_traj = plan_follow_traj(
                        Globals.robot, manip_name,
                        Globals.robot.GetLink(ee_link_name),
                        new_ee_traj[i_start:i_end + 1], old_joint_traj)
                    # (robot, manip_name, ee_link, new_hmats, old_traj):
                    part_name = {"l": "larm", "r": "rarm"}[lr]
                    bodypart2traj[part_name] = new_joint_traj
                    arms_used += lr

            ################################
            redprint(
                "Executing joint trajectory for segment %s, part %i using arms '%s'"
                % (seg_name, i_miniseg, arms_used))

            for lr in 'lr':
                set_gripper_maybesim(
                    lr,
                    binarize_gripper(seg_info["%s_gripper_joint" %
                                              lr][i_start]))
            #trajoptpy.GetViewer(Globals.env).Idle()

            if len(bodypart2traj) > 0:
                exec_traj_maybesim(bodypart2traj)

            # TODO measure failure condtions

            if not success:
                break

        redprint("Segment %s result: %s" % (seg_name, success))

        if args.fake_data_segment: break