Ejemplo n.º 1
0
def get_corresponding_data(strm1, strm2):
    """
    Returns two lists of same-length where the corresponding entries occur at the same time-stamp.
    STRM1, STRM2 are the two streams for which the corresponding entries are to be found.
    It also returns the indices at which data from both the streams is found.

    It assumes that the two streams start at the same time-scale.
    """
    dat1, dat2, inds = [],[],[]
    idx = 0
    while True:
        try:
            d1 = strm1.next()
            d2 = strm2.next()
            if d1!=None and d2!= None:
                dat1.append(d1)
                dat2.append(d2)
                inds.append(idx)
            idx += 1
        except:
            break

    strm1.reset()
    strm2.reset()

    N = len(dat1)
    print colorize("Found %s corresponding data points." % colorize(str(N), "red", True), "blue", True)
    
    return (inds, dat1, dat2)
Ejemplo n.º 2
0
    def grab_rope(self, lr, seg_samples=5):
        print colorize("TESTING FOR GRAB...", "magenta", True)
        nodes, ctl_pts = self.rope.GetNodes(), self.rope.GetControlPoints()
        graspable_inds = []
        for i in xrange(len(ctl_pts)-1):
            pts = math_utils.interp2d(np.linspace(0,1,seg_samples), [0,1], np.r_[np.reshape(ctl_pts[i], (1,3)), np.reshape(ctl_pts[i+1], (1,3))])
            if np.any([in_grasp_region(self.robot, lr, pt) for pt in pts]):
                graspable_inds.append(i)
        print colorize('graspable inds for %s: %s' % (lr, str(graspable_inds)), "magenta", True)
        if len(graspable_inds) == 0:
            return False


        robot_link = self.robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
        rope_links = self.rope.GetKinBody().GetLinks()
        for i_node in graspable_inds:
            for i_cnt in range(max(0, i_node-1), min(len(nodes), i_node+2)):
                cnt = self.bt_env.AddConstraint({
                    "type": "generic6dof",
                    "params": {
                        "link_a": robot_link,
                        "link_b": rope_links[i_cnt],
                        "frame_in_a": np.linalg.inv(robot_link.GetTransform()).dot(rope_links[i_cnt].GetTransform()),
                        "frame_in_b": np.eye(4),
                        "use_linear_reference_frame_a": False,
                        "stop_erp": .8,
                        "stop_cfm": .1,
                        "disable_collision_between_linked_bodies": True,
                    }
                })
                self.constraints[lr].append(cnt)

        return True
def publish_transform_markers(grabber, marker, T, from_frame, to_frame, rate=100):

    from visualization_msgs.msg import Marker
    from sensor_msgs.msg import PointCloud2
    
    print colorize("Transform : ", "yellow", True)
    print T
    
    marker_frame = "ar_frame"
    tf_pub = tf.TransformBroadcaster()
    pc_pub = rospy.Publisher("camera_points", PointCloud2)
    
    trans = T[0:3,3] 
    rot   = tf.transformations.quaternion_from_matrix(T)
    sleeper = rospy.Rate(10)

    while True:
        try:
            r, d = grabber.getRGBD()
            ar_tfm = get_ar_transform_id (d, r, marker)
            
            pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, from_frame)
            pc_pub.publish(pc)
            
            tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
            
            try:
                trans_m, rot_m = conversions.hmat_to_trans_rot(ar_tfm)
                tf_pub.sendTransform(trans_m, rot_m, rospy.Time.now(), marker_frame, from_frame)
            except:
                pass
            
            sleeper.sleep()
        except KeyboardInterrupt:
            break    
def get_transforms(arm, hydra, n_tfm , n_avg):
    """
    Returns a tuple of two list of N_TFM transforms, each
    the average of N_AVG transforms
    corresponding to the left/ right arm of the pr2 and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    """

    pr2_frame = 'base_footprint'
    assert arm=='right' or 'left'
    arm_frame  = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]

    hydra_frame  = 'hydra_base'
    assert hydra=='right' or hydra=='left'
    paddle_frame = 'hydra_%s'%hydra

    tf_sub = tf.TransformListener()

    I_0 = np.eye(4)
    I_0[3,3] = 0
    
    arm_tfms   = []
    hydra_tfms = []

    for i in xrange(n_tfm+1):
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm), "red", True))
        
        ## transforms which need to be averaged.
        p_ts = np.empty((0, 3))
        p_qs = np.empty((0,4))
        h_ts = np.empty((0, 3))
        h_qs = np.empty((0,4))

        sleeper = rospy.Rate(30)        
        for j in xrange(n_avg):            
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)

            ptrans, prot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
            htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))

            p_ts = np.r_[p_ts, np.array(ptrans, ndmin=2)]
            h_ts = np.r_[h_ts, np.array(htrans, ndmin=2)]
            p_qs = np.r_[p_qs, np.array(prot, ndmin=2)]
            h_qs = np.r_[h_qs, np.array(hrot, ndmin=2)]
            sleeper.sleep()
          
        ptrans_avg = np.sum(p_ts, axis=0) / n_avg
        prot_avg   = avg_quaternions(p_qs)
        htrans_avg = np.sum(h_ts, axis=0) / n_avg
        hrot_avg   = avg_quaternions(h_qs)
          
        a_tfm = conversions.trans_rot_to_hmat(ptrans_avg,prot_avg)
        h_tfm = conversions.trans_rot_to_hmat(htrans_avg,hrot_avg)
               
        arm_tfms.append(a_tfm)
        hydra_tfms.append(h_tfm)

    return (arm_tfms, hydra_tfms)
Ejemplo n.º 5
0
 def __check_time__(self, t):
     if self.t_filt == None:
         print colorize('[Filter ERROR:] Filter not initialized.', 'red', True)
         return False
         
     if self.t_filt >= t:
         print colorize('[Filter ERROR:] Observation behind the filter estimate in time. Ignoring update.', 'blue', True)
         return False
     
     return True
def publish_tf(T, from_frame, to_frame):
    print colorize("Transform : ", "yellow", True)
    print T
    tf_pub = tf.TransformBroadcaster()
    trans = T[0:3,3] 
    rot   = tf.transformations.quaternion_from_matrix(T)
    sleeper = rospy.Rate(100)

    while True:
        tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
        sleeper.sleep()
def generate_test_cmdline_params(demo_type, generate_h5s=False, sizes = [50, 25, 4], use_ik=False):
    """
    saves ~6000 sets of command line arguments.
    """
    rope_lengths     = get_rope_lengths(demo_type)
    perturb_fname    = init_perturbation_map[demo_type] + '_perturb.h5'
    perturb_fname    = osp.join(perturbations_dir, perturb_fname)
    perturb_file     = h5py.File(perturb_fname, "r") 
    subsets = split_pertubations_into_two(perturb_fname, sizes)

    if generate_h5s:
        if use_ik:
            generate_testing_h5_file_ik(demo_type, subsets[0], subsets[1])
        else:
            generate_testing_h5_files(demo_type, subsets[0], subsets[1], rope_lengths)

    cmdline_params = []
    cmdline_dir = testing_commands_dir
    if not osp.exists(cmdline_dir):
        os.mkdir(cmdline_dir)
    
    num_demos = len(rope_lengths) * np.sort(np.array(subsets[0].keys()).astype(int))
    
    num_perts = 5
    print num_perts
    
    for idx_ndemos, ndemos in enumerate(num_demos): ## x3
        for demo_set in [0,1]: ## x2
            for init_set in [0,1]: ## x2
                init_subset = subsets[init_set]
                for init_demo_idx in init_subset[np.max(init_subset.keys())]:      ## x50
                    init_config_data = perturb_file[perturb_file.keys()[init_demo_idx]]
                    init_demo_name   = perturb_file.keys()[init_demo_idx]
                    for init_perturb_name in init_config_data.keys()[0:num_perts]:                 ## x10
                        demo_data_h5_prefix = "size%d_set%d"%(ndemos, demo_set+1)
                        init_state_h5       = init_perturbation_map[demo_type]
			if len(rope_lengths) > 1: 
	                        rope_scaling_factor = sample_rope_scaling(rope_lengths)
			else:
				rope_scaling_factor = 1.0
                        results_fname       = osp.join(demo_type, "%d_demos"%ndemos, "initset%d_demoset%d"%(init_set+1, demo_set+1), "perturb_%s_%s.cp"%(init_demo_name, init_perturb_name))
                        cmdline_params.append([ndemos,
                                               demo_type,
                                               demo_data_h5_prefix,
                                               init_state_h5,
                                               str(init_demo_name),
                                               str(init_perturb_name),
                                               rope_scaling_factor,
                                               str(results_fname)])

    cmdline_file = osp.join(cmdline_dir, "%s_cmds.cp"%demo_type)
    cp.dump(cmdline_params, open(cmdline_file, 'w'))
    print colorize("wrote : %s"%cmdline_file, "yellow", True)
Ejemplo n.º 8
0
def fit_and_plot_tps(plot=False):
    
    T_cam_gt, T_hy = pickle.load(open('grp.dat', 'r'))
    
    T_cam_gt = T_cam_gt[100:len(T_cam_gt)-100]
    T_hy     = T_hy[100:len(T_hy)-100]

    BLOCK_SIZE  = 10
    N           = len(T_hy)
    train, tst  = gen_indices(N, BLOCK_SIZE)

    N_train = len(train)
    N_test  = len(tst)

    T_hy_train     = [T_hy[i] for i in train]
    T_hy_test      = [T_hy[i] for i in tst]
    T_cam_gt_train = [T_cam_gt[i] for i in train]
    T_cam_gt_test  = [T_cam_gt[i] for i in tst]      
    
    x_train    = np.empty((N_train, 3))
    x_test     = np.empty((N_test, 3)) 
    x_gt_train = np.empty((N_train, 3))
    x_gt_test  = np.empty((N_test, 3)) 
    
    for i in xrange(N_train):
        x_train[i,:] = T_hy_train[i][0:3,3]
        x_gt_train[i,:] = T_cam_gt_train[i][0:3,3]
        
    for i in xrange(N_test):
        x_test[i,:] = T_hy_test[i][0:3,3]
        x_gt_test[i,:] = T_cam_gt_test[i][0:3,3]
        
    # fit tps:
    x_gt_train = x_gt_train[100:-100]
    x_train    = x_train[100:-100]
    print colorize("Fitting TPS on %d data-points."%len(x_train), "red", True)
    f_tps = fit_tps(x_gt_train, x_train, plot=plot)

    ## fit tps on test data:
    x_pred = f_tps(x_test)
    
    ## plot:
    if plot:
        for i in xrange(3):
            plt.subplot(3,1,i+1)
            plt.plot(x_test[:,i],label='hydra')
            plt.plot(x_gt_test[:,i],label='cam')
            plt.plot(x_pred[:,i],label='pred')
            plt.legend()
        plt.show()
    def process_request(self, req):
        if req['type'] == 'custom':
            try:
                f = self.plotting_funcs[req['func']]        
            except KeyError:
                print colorize("No custom plotting function with name : %s. Ignoring plot request."%req['func'], "red")

        elif req['type'] == 'mlab':
            try:
                f = getattr(mlab, req['func'])
            except KeyError:
                print colorize("No mlab plotting function with name : %s. Ignoring plot request."%req['func'], "red")
        
        args, kwargs = req['data']
        f(*args, **kwargs)
Ejemplo n.º 10
0
def get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs):
    """
    Return the first state (mean, covar) to initialize the kalman filter with.
    Assumes that the time-stamps start at a common zero (are on the same time-scale).
    
    Returns a state b/w t=[0, dt]
    
    Gives priority to AR-markers. If no ar-markers are found in [0,dt], it returns
    hydra's estimate but with larger covariance.
    """
    
    ar1 = [c1_tfs[i] for i in xrange(len(c1_ts)) if c1_ts[i] <= dt]
    ar2 = [c2_tfs[i] for i in xrange(len(c2_ts)) if c2_ts[i] <= dt]
    hy =  [hy_tfs[i] for i in xrange(len(hy_ts)) if hy_ts[i] <= dt] 
    
    if ar1 != [] or ar2 != []:
        ar1.extend(ar2)
        x0 =  state_from_tfms_no_velocity([avg_transform(ar1)])
        I3 = np.eye(3)
        S0 = scl.block_diag(1e-3*I3, 1e-2*I3, 1e-3*I3, 1e-3*I3)
    else:
        assert len(hy)!=0, colorize("No transforms found for KF initialization. Aborting.", "red", True)
        x0 = state_from_tfms_no_velocity([avg_transform(hy)])
        I3 = np.eye(3)
        S0 = scl.block_diag(1e-1*I3, 1e-1*I3, 1e-2*I3, 1e-2*I3)
    return (x0, S0)
def publish_transform(T, from_frame, to_frame, rate=100):
    
    print colorize("Transform : ", "yellow", True)
    print T
    
    tf_pub = tf.TransformBroadcaster()
    
    trans, rot = conversions.hmat_to_trans_rot(T)
    sleeper = rospy.Rate(10)

    while True:
        try:            
            tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
            sleeper.sleep()
        except KeyboardInterrupt:
            break    
def get_transforms(arm, hydra, n_tfm , n_avg):
    """
    Returns a tuple of two list of N_TFM transforms, each
    the average of N_AVG transforms
    corresponding to the left/ right arm of the pr2 and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    
    return arm_tfms, hydra_tfms
    """

    pr2_frame = 'base_footprint'
    assert arm == 'right' or 'left'
    arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]

    hydra_frame = 'hydra_base'
    assert hydra == 'right' or hydra == 'left'
    paddle_frame = 'hydra_%s' % hydra

    tf_sub = tf.TransformListener()

    I_0 = np.eye(4)
    I_0[3, 3] = 0
    
    gripper_tfms = []
    hydra_tfms = []

    for i in xrange(n_tfm):
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm - 1), "red", True))
        
        # # transforms which need to be averaged.
        gtfms = []
        htfms = []
        
        sleeper = rospy.Rate(30)        
        for j in xrange(n_avg):            
            print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True)

            gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
            gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot))
            htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
            htfms.append(conversions.trans_rot_to_hmat(htrans, hrot))
            sleeper.sleep()
            
        gripper_tfms.append(avg_transform(gtfms))
        hydra_tfms.append(avg_transform(htfms))

    return (gripper_tfms, hydra_tfms)
Ejemplo n.º 13
0
def get_tool_model(n_tfm, n_avg):

    listener = tf.TransformListener()

    gripper_frame = 'l_gripper_tool_frame'
    base_frame = 'base_link'
    model_frame = ''
    depth_frame = 'camera_depth_optical_frame'
    head_frame = 'head_plate_frame'
    model_tfms = []
    tool_tfms = []
    

    for i in xrange(n_tfm):
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i + 1, n_tfm), "red", True))
        tool_trans_list = np.empty((0, 3))
        tool_quat_list = np.empty((0, 4))
        marker_trans_list = np.empty((0, 3))
        marker_quat_list = np.empty((0, 4))
        head_trans_list = np.empty((0, 3))
        head_quat_list = np.empty((0, 4))
	time.sleep(3)
	for j in xrange(n_avg):

            tool_trans, tool_quat, model_trans, model_quat, head_trans, head_quat = None, None, None, None, None, None

            while tool_trans == None:
                #model_trans, model_quat = listener.lookupTransform(base_frame, model_frame, rospy.Time(0))
                tool_trans, tool_quat = listener.lookupTransform(base_frame, gripper_frame, rospy.Time(0))
                head_trans, head_quat = listener.lookupTransform(base_frame, head_frame, rospy.Time(0))

            tool_trans_list = np.r_[tool_trans_list, np.array(tool_trans, ndmin=2)]
            tool_quat_list = np.r_[tool_quat_list, np.array(tool_quat, ndmin=2)]
            #model_trans_list = np.r_[model_trans_list, np.array(model_trans, ndmin=2)]
            #model_quat_list = np.r_[model_quat_list, np.array(model_quat, ndmin=2)]
            head_trans_list = np.r_[head_trans_list, np.array(head_trans, ndmin=2)]
            head_quat_list = np.r_[head_quat_list, np.array(head_quat, ndmin=2)]
        
        tool_trans_avg = np.sum(tool_trans_list, axis=0) / n_avg
        tool_quat_avg = avg_quaternions(tool_quat_list)
        #model_trans_avg = np.sum(model_trans_list, axis=0) / n_avg
        #model_quat_avg = avg_quaternions(model_quart_list)
        head_trans_avg = np.sum(head_trans_list, axis=0) / n_avg
        head_quat_avg = avg_quaternions(head_quat_list)
    
        tool_tfm = conversions.trans_rot_to_hmat(tool_trans_avg, tool_quat_avg)

        head_tfm = conversions.trans_rot_to_hmat(head_trans_avg, head_quat_avg)
        #depth_to_model_tfm = conversions.trans_rot_to_hmat(model_trans_avg, model_quat_avg)
        depth_tfm = head_tfm.dot(T_h_k)
        #model_tfm = depth_tfm.dot(depth_to_model_tfm)
        
        tool_tfms.append(tool_tfm)
        #model_tfms.append(model_tfm)
    #return tool_tfms, model_tfms
    return tool_tfms
Ejemplo n.º 14
0
def call_on_cloud(cmd_params, core_type, num_batches, start_batch_num, end_batch_num):
    ntests = len(cmd_params)
    batch_size = int(math.ceil(ntests/(num_batches+0.0)))

    batch_edges = batch_size*np.array(xrange(num_batches))[start_batch_num : end_batch_num]
    print batch_edges
    for i in xrange(len(batch_edges)):
        if i==len(batch_edges)-1:
            cmds = cmd_params[batch_edges[i]:]
        else:
            cmds = cmd_params[batch_edges[i]:min(batch_edges[i+1], len(cmd_params))]
        print colorize("calling on cloud..", "yellow", True)
        try:
            jids = cloud.map(run_sim_test, cmds, _vol='rss_dat', _env='RSS3', _type=core_type)
            res  = cloud.result(jids)
            print colorize("got results for batch %d/%d "%(i, len(batch_edges)), "green", True)
            save_results(res)
        except Exception as e:
            print "Found exception %s. Not saving data for this demo."%e
Ejemplo n.º 15
0
    def __init__(self):
        self.finished = False
        devices = glob.glob("/dev/ttyACM*")
        if len(devices) ==0:
            raise RuntimeError(colorize( "Arduino not connected.", 'red', True))
        elif len(devices) >1:
            print colorize("Found more than one matching devices. Enter the number of the one which is arduino:", 'blue', True)
            for n, d in enumerate(devices):
                print "\t%d. %s"%(n,d)
                while True:
                    try:
                        sel = int(raw_input())
                        if 0<= sel < len(devices):
                            break  
                    except:
                        print "\tPlease enter a number in the range [0, %d]" % len(devices)
                        pass
                arduino = devices[sel]
        else:           
            arduino = devices[0]
        
        try:
            self.ser = serial.Serial(arduino, 9600, timeout=1)
        except serial.SerialException as e:
            raise RuntimeError(colorize("Error opening serial port '{}': {}".format(arduino, e), "red", True))

        self.poll_thread = Thread(target=self.poll_arduino)
        self.poll_thread.daemon = True
        self.poll_thread.start()
        
        time.sleep(0.1)
        print colorize("Arduino all set.", "green", True)
Ejemplo n.º 16
0
def plot_warping(f, src, target, fine=True, draw_plinks=True):
    """
    function to plot the warping as defined by the function f.
    src    : nx3 array
    target : nx3 array
    fine   : if fine grid else coarse grid.
    """
    print colorize("Plotting grid ...", "blue", True)
    mean = np.mean(src, axis=0)
    mins = np.min(src, axis=0)
    maxes = np.max(src, axis=0)

    grid_lines = []
    if fine:
        grid_lines = gen_grid2(f, mins=mins, maxes=maxes, xres=0.005, yres=0.005, zres=0.002)
    else:
        grid_lines = gen_grid(f, mins=mins, maxes=maxes)

    plotter_requests = []
    plotter_requests.append(gen_mlab_request(mlab.clf))
    plotter_requests.append(gen_custom_request("lines", lines=grid_lines, color=(0, 0.5, 0.3)))

    warped = f(src)

    plotter_requests.append(
        gen_mlab_request(mlab.points3d, src[:, 0], src[:, 1], src[:, 2], color=(1, 0, 0), scale_factor=0.001)
    )
    plotter_requests.append(
        gen_mlab_request(mlab.points3d, target[:, 0], target[:, 1], target[:, 2], color=(0, 0, 1), scale_factor=0.001)
    )
    plotter_requests.append(
        gen_mlab_request(mlab.points3d, warped[:, 0], warped[:, 1], warped[:, 2], color=(0, 1, 0), scale_factor=0.001)
    )

    if draw_plinks:
        plinks = [np.c_[ps, pw].T for ps, pw in zip(src, warped)]
        plotter_requests.append(gen_custom_request("lines", lines=plinks, color=(0.5, 0, 0), line_width=2, opacity=1))

    return plotter_requests
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        avg_tfms = {}
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)            

            tfms = {}
            tfms.update(gmt.get_ar_markers_from_cameras(self.cameras, parent_frame=self.parent_frame, markers=self.ar_markers))
            tfms.update(gmt.get_hydra_transforms(parent_frame=parent_frame, hydras = self.hydras))

            pot_angle = gmt.get_pot_angle()
                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
        
        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
Ejemplo n.º 18
0
def call_pnp_app(demo_type):
    demo_testing_dir = osp.join(testing_results_dir, demo_type)
    sub    = ['12_demos', '75_demos', '150_demos']
    subsub = ['initset1_demoset1', 'initset1_demoset2', 'initset2_demoset1', 'initset2_demoset2']
    
    results = {}
    
    for s in sub:
        res_ss = {}
        for ss in subsub:
            snapshot_path = osp.join(demo_testing_dir, s, ss, 'snapshots')
            save_path     = '/tmp/%d.cp'%np.random.randint(10000000000)
            print colorize('doing : %s/%s'%(s,ss), "green", True)            
            subprocess.call('python /home/ankush/sandbox444/human_demos/hd_evaluate/pnpApp/pnpApp.py --snapshot_path=%s --save_path=%s'%(snapshot_path, save_path), shell=True)
            stats = cp.load(open(save_path,'r'))
            res_ss[ss] = stats
        results[s] = res_ss
    
    stats_file = osp.join(demo_testing_dir, 'stats.cp')
    
    with open(stats_file,'w') as f: 
        cp.dump(results, f)
    print colorize("all done", "red", True)
def split_pertubations_into_two(perturb_fname, sizes = [50, 25, 4]):
    perturb_dat  = h5py.File(perturb_fname, 'r')
    num_perturbs = len(perturb_dat.keys())

    assert num_perturbs >= 90, colorize("not enough demos in the perturbation file : %s"%perturb_fname)

    subset1, subset2 = {}, {}
    for i in xrange(len(sizes)):
        if i==0:
            random_indices = range(num_perturbs)
            np.random.shuffle(random_indices)
            subset1[sizes[i]] = random_indices[:sizes[i]]
            subset2[sizes[i]] = random_indices[sizes[i]:]
        else:
            subset1[sizes[i]] = [subset1[sizes[i-1]][x] for x in np.random.choice(range(len(subset1[sizes[i-1]])), sizes[i], replace=False)]
            subset2[sizes[i]] = [subset2[sizes[i-1]][x] for x in np.random.choice(range(len(subset2[sizes[i-1]])), sizes[i], replace=False)] 

    perturb_dat.close()
    return subset1, subset2
def generate_testing_h5_file_ik(demo_type, subset1, subset2):
    """
    demo_type : the type of the knot : {overhand, square-knot, ...}
    subset1, subset2 : the splitting indices as returned by split_perturbations_into_two above
    """
    def copy_to_subset(parent_h5, child_h5, subset):
        for idx in subset:
            if idx >= len(parent_h5.keys()):
                continue
            pkey = parent_h5.keys()[idx]
            parent_h5.copy(pkey, child_h5)

    rope_lengths = [140]
    demo_h5_fnames = {l : osp.join(demo_files_dir, "%s"%(demo_type),  "%s.h5"%(demo_type)) for l in [140]}
    demo_h5_files  = {}

    for h5_fname in demo_h5_fnames.values():
        if not osp.exists(h5_fname):
            print colorize(" %s : does not exist. Exiting..."%h5_fname, "red", True)
            sys.exit(-1)

    for l in rope_lengths:
        demo_h5_files[l] = h5py.File(demo_h5_fnames[l], 'r')

    out_dir = osp.join(data_dir, "testing_h5s", demo_type)
    if not osp.exists(out_dir):
        os.mkdir(out_dir)
    
    num_lengths  = len(rope_lengths)
    subset_sizes = np.array([np.sort(np.array(subset1.keys()))])

    cumm_subset_sizes = num_lengths * subset_sizes.astype(int)
    for i,cumm_size in enumerate(cumm_subset_sizes):
        for l in rope_lengths:
            subset1_h5_name, subset2_h5_name = [osp.join(out_dir, "size%d_set%d.h5"%(cumm_size, x)) for x in [1,2]]
            
            subset1_h5_file = h5py.File(subset1_h5_name, 'w')
            subset2_h5_file = h5py.File(subset2_h5_name, 'w')

            copy_to_subset(demo_h5_files[l], subset1_h5_file, subset1[np.sort(subset1.keys())[i]])            
            copy_to_subset(demo_h5_files[l], subset2_h5_file, subset2[np.sort(subset2.keys())[i]])

            subset1_h5_file.close()
            subset2_h5_file.close()
            print colorize("saved : %s"%subset1_h5_name, "green", True)            
            print colorize("saved : %s"%subset2_h5_name, "green", True)

    for h5file in demo_h5_files.values():
        h5file.close()
Ejemplo n.º 21
0
def test_all(stop=False):
    nPass,nFail = 0,0
    for (name,func) in TEST_FUNCS.items():
        print colorize("function: %s"%name,"green")
        try:
            t_start = time()
            func()
            t_elapsed = time() - t_start
            print colorize("PASSED (%.3f sec)"%t_elapsed,"blue")
            nPass += 1
        except Exception:    
            traceback.print_exc(file=sys.stdout)
            if stop: raise
            print colorize("FAILED","red")
            nFail += 1
            
            
    print "%i passed, %i failed"%(nPass,nFail)
def create_graph_from_observations(parent_frame, calib_info, min_obs=5, n_avg=5, freq=None):
    """
    Runs a loop till graph has enough data and user is happy with data.
    Or run with frequency specified until enough data is gathered.
    
    @parent_frame -- frame to get observations in.
    @num_markers -- total_number of markers.
    @calib_info -- dict with information on what groups and markers to search for.
                   also gives information on which is the master group and how the angle affects group.
    @min_obs -- minimum number of observations for transform required after finding it once.
    @n_avg -- number of times to average transform per observation.
    """
    global tf_listener
    if rospy.get_name() == "/unnamed":
        rospy.init_node("gripper_marker_calibration")
    tf_listener = tf.TransformListener()
    tf_listener.clear()

    masterGraph = nx.DiGraph()
    ar_markers = []
    hydras = []
    ps_markers = []

    # Setup the groups
    for group in calib_info:
        masterGraph.add_node(group)
        masterGraph.node[group]["graph"] = nx.Graph()
        masterGraph.node[group]["angle_scale"] = calib_info[group]["angle_scale"]

        if calib_info[group].get("ar_markers") is None:
            calib_info[group]["ar_markers"] = []
        if calib_info[group].get("hydras") is None:
            calib_info[group]["hydras"] = []
        if calib_info[group].get("ps_markers") is None:
            calib_info[group]["ps_markers"] = []

        if calib_info[group].get("master_group") is not None:
            masterGraph.node[group]["master"] = 1
            masterGraph.node[group]["angle_scale"] = 0

        masterGraph.node[group]["markers"] = (
            calib_info[group]["ar_markers"] + calib_info[group]["hydras"] + calib_info[group]["ps_markers"]
        )
        masterGraph.node[group]["calib_info"] = calib_info[group]
        ar_markers.extend(calib_info[group]["ar_markers"])
        hydras.extend(calib_info[group]["hydras"])
        ps_markers.extend(calib_info[group]["ps_markers"])

    if freq is not None:
        wait_time = 5
        print "Waiting for %f seconds before collecting data." % wait_time
        time.sleep(wait_time)

    sleeper = rospy.Rate(30)
    count = 0
    while True:
        if freq is None:
            raw_input(colorize("Iteration %d: Press return when ready to capture transforms." % count, "red", True))
        else:
            print colorize("Iteration %d" % count, "red", True)

        avg_tfms = {}
        for j in xrange(n_avg):
            print colorize("\tGetting averaging transform : %d of %d ..." % (j, n_avg - 1), "blue", True)

            tfms = {}
            tfms.update(get_ar_transforms(ar_markers, parent_frame))
            tfms.update(get_hydra_transforms(hydras, parent_frame))
            tfms.update(get_phasespace_transforms(ps_markers, parent_frame))

            pot_angle = get_potentiometer_angle()

            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])

            sleeper.sleep()

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(masterGraph, avg_tfms, pot_angle)

        count += 1
        if is_ready(masterGraph, min_obs):
            if freq:
                break
            elif not yes_or_no("Enough data has been gathered. Would you like to gather more data anyway?"):
                break

    print "Finished gathering data in %d iterations." % count
    print "Calibrating for optimal transforms between markers..."
    G_opt = compute_relative_transforms(masterGraph, min_obs=min_obs)
    print "Finished calibrating."
    return G_opt
Ejemplo n.º 23
0
def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef):
    
    N,D = x_na.shape
    M = nr_ma.shape[0]
    E = N + 4*M
    F = E - M
    Q = N + 3*M - 4
    
    s = .1 # tetrahedron sidelength (meters)
    u = 1/(2*np.sqrt(2))
    
    tetra_pts = []
    for pt in nr_ma:
        tetra_pts.append(s*np.r_[-.5, 0, -u]+pt)
        tetra_pts.append(s*np.r_[+.5, 0, -u]+pt)
        tetra_pts.append(s*np.r_[0, -.5, +u]+pt)
        tetra_pts.append(s*np.r_[0, +.5, +u]+pt)
    
    x_ea = np.r_[x_na, tetra_pts]

    badsub_ex = np.c_[x_ea, np.ones((E,1)), np.r_[np.zeros((N,M)), np.repeat(np.eye(M), 4, axis=0)]]    
    lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng,  bend_coef, 1e-3)
    w_eg = np.r_[w_ng, np.zeros((4*M, D))]

    assert badsub_ex.shape[0] >= badsub_ex.shape[1]
    _u,_s,_vh = np.linalg.svd(badsub_ex, full_matrices=True)
    assert badsub_ex.shape[1] == _s.size
    N_eq = _u[:,badsub_ex.shape[1]:] # null of data
        
    assert N_eq.shape == (E,Q)

    assert E == N + 4*M
    assert F == Q + 4
    # e is number of kernels
    # q is number of nonrigid dofs
    # f is total number of dofs
    K_ee = tps_kernel_matrix(x_ea)
    K_ne = K_ee[:N, :]
    Q_nf = np.c_[x_na, np.ones((N,1)),K_ne.dot(N_eq)]
    QQ_ff = np.dot(Q_nf.T, Q_nf)
    Bend_ff = np.zeros((F,F))
    Bend_ff[4:, 4:] = - N_eq.T.dot(K_ee.dot(N_eq)) # K_qq
    
    assert Q_nf.shape == (N, F)
    assert w_eg.shape == (E, D)
    
    n_iter=40
    for i in xrange(n_iter):
        
        
        # if plotting and i%plotting==0:
            # import lfd.registration as lr
            # lr.Globals.setup()
            # def eval_partial(x_ma):
            #     return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) 
            # lr.plot_orig_and_warped_clouds(eval_partial, x_na, y_ng, res=.008)            
        
        X_fg = np.r_[lin_ag, 
                    trans_g[None,:], 
                    N_eq.T.dot(w_eg)]

        res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True)
        if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'),
        if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost)
                
        
        Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma, lin_ag, trans_g, w_eg, x_ea, return_tuple=True)
        nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea)        
        
        fullstep_fg = np.empty((F,D))
        for g in xrange(D):
            J_zf = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zeg[:,g::D].dot(N_eq)]
            JJ_ff = np.dot(J_zf.T, J_zf)
            A_ff = nr_coef*JJ_ff + QQ_ff + bend_coef*Bend_ff
            X0 = X_fg[:,g]
            B_f = nr_coef*np.dot(J_zf.T, np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot(y_ng[:,g])
            fullstep_fg[:,g] = np.linalg.solve(A_ff,B_f) - X_fg[:,g]

        cost_improved = False
        for stepsize in 3.**np.arange(0,-10,-1):
            cand_X_fg = X_fg + fullstep_fg*stepsize
            cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[D], N_eq.dot(cand_X_fg[D+1:])
            fval_cand = tps_nr_cost_eval_general(cand_lin_ag, cand_trans_g, cand_w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=False)
            if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand)
            if fval_cand < fval:
                cost_improved = True
                break
        if not cost_improved:
            if VERBOSE: print "couldn't improve objective"
            break

            
        lin_ag = cand_lin_ag
        trans_g = cand_trans_g
        w_eg = cand_w_eg
    return lin_ag, trans_g, w_eg, x_ea
Ejemplo n.º 24
0
def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
    N,D = x_na.shape
    lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
    #return lin_ag, trans_g, w_ng

    ##for testing that it takes one step when nonrigidity cost is zero:
    #lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0)
    #res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
    #print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
    #lin_ag += np.random.randn(*lin_ag.shape)*5
    #res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
    #print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
    
    _u,_s,_vh = np.linalg.svd(np.c_[x_na, np.ones((N,1))], full_matrices=True)
    N_nq = _u[:,4:] # null of data
    #w_ng = N_nq.dot(N_nq.T.dot(w_ng))
        
    K_nn = tps_kernel_matrix(x_na)
    Q_nn = np.c_[x_na, np.ones((N,1)),K_nn.dot(N_nq)]
    QQ_nn = np.dot(Q_nn.T, Q_nn)
    Bend_nn = np.zeros((N,N))
    Bend_nn[4:, 4:] = - N_nq.T.dot(K_nn.dot(N_nq))
    
    n_iter=60
    for i in xrange(n_iter):
        X_ng = np.r_[lin_ag, trans_g[None,:], N_nq.T.dot(w_ng)]

        res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True)
        if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'),
        if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost)
                
        
        Jl_zcg, Jt_zg, Jw_zng = tps_nr_grad(nr_ma, lin_ag, trans_g, w_ng, x_na, return_tuple=True)
        nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na)
        
        
        if method == "newton":
            fullstep_ng = np.empty((N,D))
            for g in xrange(D):
                J_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
                JJ_nn = np.dot(J_zn.T, J_zn)
                A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn
                X0 = X_ng[:,g]
                B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g])
                fullstep_ng[:,g] = np.linalg.solve(A,B) - X_ng[:,g]

        elif method == "gradient":
            # def eval_partial(cand_X_ng):
            #     cand_X_ng = cand_X_ng.reshape(-1,3)
            #     cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
            #     fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
            #     return fval_cand
            # def eval_partial2(cand_X_ng):
            #     return ((Q_nn.dot(X_ng) - y_ng)**2).sum()
            # def eval_partial3(cand_X_ng):
            #     cand_X_ng = cand_X_ng.reshape(-1,3)
            #     cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
            #     return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g, cand_w_ng, x_na))**2).sum()
            
            
            grad_ng = np.empty((N,D))
            for g in xrange(D-1,-1,-1):
                Jnr_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
                grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \
                    + 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \
                    + 2 * bend_coef * Bend_nn.dot(X_ng[:,g])

            #assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng))
            #assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng))
            #grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3)
            fullstep_ng = -grad_ng
            #assert np.allclose(grad0_ng, grad_ng)
            
            
            

        cost_improved = False
        for stepsize in 3.**np.arange(0,-10,-1):
            cand_X_ng = X_ng + fullstep_ng*stepsize
            cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
            fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
            if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand)
            if fval_cand < fval:
                cost_improved = True
                break
        if not cost_improved:
            if VERBOSE: print "couldn't improve objective"
            break

            
        lin_ag = cand_lin_ag
        trans_g = cand_trans_g
        w_ng = cand_w_ng
    return lin_ag, trans_g, w_ng
def get_transform_freq (grabber, marker, freq=0.5, n_avg=10, n_tfm=3, print_shit=False):
    """
    Stores data at frequency provided.
    Switch on phasespace before this.
    """
    tfms_ar = []
    tfms_ph = []

    I_0 = np.eye(4)
    I_0[3,3] = 0
    
    i = 0
    
    wait_time = 5
    print "Waiting for %f seconds before collecting data."%wait_time
    time.sleep(wait_time)
    
    
    avg_freq = 30
    while i < n_tfm:
        print "Transform %i"%(i+1)
        
        j = 0
        ar_tfms = []
        ph_tfms = []
        while j < n_avg:
            rgb, depth = grabber.getRGBD()
            ar_tfm = get_ar_transform_id(depth, rgb, marker)
            ph_tfm = ph.marker_transform(0,1,2, ph.get_marker_positions())

            if ar_tfm is None: 
                print colorize("Could not find AR marker %i."%marker,"red",True)
                continue
            if ph_tfm is None:
                print colorize("Could not correct phasespace markers.", "red", True)
                continue

            print colorize("Got transform %i for averaging."%(j+1), "blue", True)
            ar_tfms.append(ar_tfm)
            ph_tfms.append(ph_tfm)
            j += 1
            time.sleep(1/avg_freq)
        
        ar_avg_tfm = utils.avg_transform(ar_tfms)
        ph_avg_tfm = utils.avg_transform(ph_tfms)
        
        if print_shit:
            print "\n ar: "
            print ar_avg_tfm
            print ar_avg_tfm.dot(I_0).dot(ar_avg_tfm.T)
            print "ph: "
            print ph_avg_tfm
            print ph_avg_tfm.dot(I_0).dot(ph_avg_tfm.T)
            
        
        tfms_ar.append(ar_avg_tfm)
        tfms_ph.append(ph_avg_tfm)
        i += 1
        time.sleep(1/freq)
    
    print "Found %i transforms. Calibrating..."%n_tfm
    Tas = ss.solve4(tfms_ar, tfms_ph)
    print "Done."
    
    T_cps = [tfms_ar[i].dot(Tas).dot(np.linalg.inv(tfms_ph[i])) for i in xrange(len(tfms_ar))]
    return utils.avg_transform(T_cps)
def get_transforms(arm, hydra, n_tfm , n_avg):
    """
    Returns a tuple of two list of N_TFM transforms, each
    the average of N_AVG transforms
    corresponding to the left/ right arm of the pr2 and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    
    Return gripper_tfms, hydra_tfms, kinect_tfms, head_tfms
    """

    pr2_frame = 'base_footprint'
    assert arm == 'right' or 'left'
    arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]
    head_frame = 'head_plate_frame'
    hydra_frame = 'hydra_base'
    assert hydra =='right' or hydra=='left'
    paddle_frame = 'hydra_%s' % hydra

    tf_sub = tf.TransformListener()

    I_0 = np.eye(4)
    I_0[3, 3] = 0
    
    gripper_tfms = []
    hydra_tfms = []
    kinect_tfms = []
    head_tfms = []
    ar_markers = ARMarkersRos('/camera1_')
    time.sleep(3)
    
    
    marker_id = 11
    i = 0
    while i < n_tfm:
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm-1), "red", True))
       
        ## transforms which need to be averaged.
        gtfms = []
        htfms = []
        ktfms = []
        ptfms = []

        sleeper = rospy.Rate(30)  
        collect_enough_data = True      
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j, n_avg-1), "blue", True)
            
            kinect_tfm = ar_markers.get_marker_transforms(time_thresh=0.5)
            print kinect_tfm
            if kinect_tfm == {}:
                print "Lost sight of AR marker. Breaking..."
                collect_enough_data = False
                break
            
            ptrans, prot = tf_sub.lookupTransform(pr2_frame, head_frame, rospy.Time(0))
            ptfms.append(conversions.trans_rot_to_hmat(ptrans,prot))   
            gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
            gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot))
            htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
            htfms.append(conversions.trans_rot_to_hmat(htrans, hrot))
            ktrans, krot = conversions.hmat_to_trans_rot(kinect_tfm[marker_id])
            ktfms.append(conversions.trans_rot_to_hmat(ktrans, krot))
            sleeper.sleep()
            
        if collect_enough_data:
            gripper_tfms.append(avg_transform(gtfms))
            hydra_tfms.append(avg_transform(htfms))
            kinect_tfms.append(avg_transform(ktfms))
            head_tfms.append(avg_transform(ptfms))
            
            i += 1

    return (gripper_tfms, hydra_tfms, kinect_tfms, head_tfms)
Ejemplo n.º 27
0
def get_synced_data(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera, plot=False):
    """
    Returns two lists : (1) estimate of the hydra-sensor from hydra-base.
                        (2) estimate of the hydra-sensor from the camera in hydra-bases frame.
    """
    
    ## The hydra and camera data streams are not synced (some delay issues), therefore
    ## first remove that shift using correlation shift.
    _, _, _, ar1_strm, ar2_strm, hy_strm = relative_time_streams(data_file, lr, freq, single_camera)    
    
    ## run the kalman filter on just the camera-data. 
    nsteps, tmin, F_means, S, A,R = run_kalman_filter(data_file, lr, freq, use_spline, False, single_camera)

    X_kf = np.array(F_means)
    X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T

    if use_spline:
        smooth_hy = (t for t in fit_spline_to_stream(hy_strm, nsteps))
    else:
        smooth_hy = hy_strm
                 
    indices_ar1 = []
    indices_ar2 = []
    indices_hy  = []
    Ts_ar1      = []
    Ts_ar2      = []
    Ts_hy       = []

    for i in xrange(nsteps):
        ar1_est = soft_next(ar1_strm)
        ar2_est = soft_next(ar2_strm)
        hy_est  = soft_next(smooth_hy)

        if ar1_est != None:
            Ts_ar1.append(ar1_est)
            indices_ar1.append(i)
        if ar2_est != None:
            Ts_ar2.append(ar2_est)
            indices_ar2.append(i)
        if hy_est != None:
            Ts_hy.append(hy_est)
            indices_hy.append(i)

    X_ar1 = state_from_tfms_no_velocity(Ts_ar1).T
    X_ar2 = state_from_tfms_no_velocity(Ts_ar2).T
    X_hy  = state_from_tfms_no_velocity(Ts_hy).T

    ## shift the ar1 data to sync it up with the hydra data:    
    if customized_shift != None:
        shift = customized_shift
    else:
        shift = correlation_shift(X_kf, X_hy)
    
    ## shift the indices of ar1 by the SHIFT calculated above:
    indices_hy = [idx + shift for idx in indices_hy]
    indices_hy = [x for x in indices_hy if 0<= x and x < nsteps]

    Ts_hy_matching  = []
    Ts_ar1_matching = []
    for i,x in enumerate(indices_ar1):
        try:
            idx = indices_hy.index(x)
            Ts_hy_matching.append(Ts_hy[idx])
            Ts_ar1_matching.append(Ts_ar1[i])
        except:
            pass

    print colorize("Found %s corresponding data points b/w camera and hydra." % colorize("%d"%len(Ts_ar1_matching), "red", True), "blue", True)

    if plot:
        X_ar_matching = state_from_tfms_no_velocity(Ts_ar1_matching).T
        X_hy_matching = state_from_tfms_no_velocity(Ts_hy_matching).T    
        plot_kalman_core(X_ar_matching, X_hy_matching, None, None, None, None, None, None, 'fs')
        plt.show()
    
    return (Ts_hy_matching, Ts_ar1_matching)
Ejemplo n.º 28
0
def get_transforms(marker, hydra, n_tfm , n_avg):
    """
    Returns a tuple of two list of N_TFM transforms, each
    the average of N_AVG transforms
    corresponding to the AR marker with ID = MARKER and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    """

    camera_frame = 'camera1_link'
    hydra_frame = 'hydra_base'
    marker_frame = 'ar_marker%d_camera1' % marker    
    assert hydra == 'right' or hydra == 'left'
    paddle_frame = 'hydra_%s' % hydra

    tf_sub = tf.TransformListener()

    I_0 = np.eye(4)
    I_0[3, 3] = 0
    
    ar_tfms = []
    hydra_tfms = []
    
    
    for i in xrange(n_tfm + 1):
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm), "red", True))
        
        # # transforms which need to be averaged.
        m_ts = np.empty((0, 3))
        m_qs = np.empty((0, 4))
        h_ts = np.empty((0, 3))
        h_qs = np.empty((0, 4))
        
        for j in xrange(n_avg):            
            print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True)
            
            mtrans, mrot, htrans, hrot = None, None, None, None

            sleeper = rospy.Rate(30)
            while htrans == None:
                # now = rospy.Time.now()
                # tf_sub.waitForTransform(camera_frame, marker_frame, now)
#                 mtrans, mrot = tf_sub.lookupTransform(marker_frame, camera_frame, rospy.Time(0))
#                 (htrans,hrot) = tf_sub.lookupTransform(paddle_frame, hydra_frame, rospy.Time(0))
                mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0))
                htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
                sleeper.sleep()
                # try:
                #    tf_sub.waitForTransform(hydra_frame, paddle_frame, now, rospy.Duration(0.05))
                # except (tf.LookupException):
                #    continue

            m_ts = np.r_[m_ts, np.array(mtrans, ndmin=2)]
            h_ts = np.r_[h_ts, np.array(htrans, ndmin=2)]
            m_qs = np.r_[m_qs, np.array(mrot, ndmin=2)]
            h_qs = np.r_[h_qs, np.array(hrot, ndmin=2)]
          
        mtrans_avg = np.sum(m_ts, axis=0) / n_avg
        mrot_avg = avg_quaternions(m_qs)
        htrans_avg = np.sum(h_ts, axis=0) / n_avg
        hrot_avg = avg_quaternions(h_qs)
          
#         ar_tfm = tf_sub.fromTranslationRotation(mtrans_avg, mrot_avg)
#         h_tfm = tf_sub.fromTranslationRotation(htrans_avg, hrot_avg)
        ar_tfm = conversions.trans_rot_to_hmat(mtrans_avg, mrot_avg)
        h_tfm = conversions.trans_rot_to_hmat(htrans_avg, hrot_avg)
         
        print "\nar:"
        print ar_tfm
        print ar_tfm.dot(I_0).dot(ar_tfm.T)
        print "h:"
        print h_tfm
        print h_tfm.dot(I_0).dot(h_tfm.T), "\n"
         
#         ar_tfm = conversions.trans_rot_to_hmat(mtrans,mrot)
#         h_tfm = conversions.trans_rot_to_hmat(htrans,hrot)
#         print "\nar:"
#         print ar_tfm
#         print ar_tfm.dot(I_0).dot(ar_tfm.T)
#         print "h:"
#         print h_tfm
#         print h_tfm.dot(I_0).dot(h_tfm.T), "\n"
        
        ar_tfms.append(ar_tfm)
        hydra_tfms.append(h_tfm)
        
    return (ar_tfms, hydra_tfms)
Ejemplo n.º 29
0
plt.clf()

xpf_t_2 = xpf_2.T
xh_t_2  = xh_2.T

for i in xrange(6):
    j = i+3 if i > 2 else i

    plt.subplot(3,2,i+1)
    plt.plot(xpf_t_2[j,:], label='pr2')
    plt.plot(xh_t_2[j,:], label='hydra')
    plt.plot(X_est[j, :], label='estimate')
    plt.ylabel(axlabels[i])
    plt.legend()
plt.show(block=False)


orig_cal_pose_error = ec.calc_pose_error(A, B)
print colorize("original camera calibration pose error", "blue", True)
print "mean", np.mean(orig_cal_pose_error, axis=0)
print "std", np.std(orig_cal_pose_error, axis=0)
print colorize("--------------------------------------", "blue", True)


# systematic and GP corrected robot poses
gp_pose_error = ec.calc_pose_error(D, ests)
print colorize("systematic and GP corrected pose error", "blue", True)
print "mean", np.mean(gp_pose_error, axis=0)
print "std", np.std(gp_pose_error, axis=0)
print colorize("--------------------------------------", "blue", True)
def get_transform_ros(marker, n_tfm, n_avg, freq=None):
    """
    Returns a tuple of two list of N_TFM transforms, each
    the average of N_AVG transforms
    corresponding to the AR marker with ID = MARKER and 
    the phasespace markers.
    """

    camera_frame = 'camera_depth_optical_frame'
    marker_frame = 'ar_marker_%d'%marker
    ps_frame = 'ps_marker_transform'

    tf_sub = tf.TransformListener()

    I_0 = np.eye(4)
    I_0[3,3] = 0
    
    ar_tfms    = []
    ph_tfms = []

    wait_time = 5
    print "Waiting for %f seconds before collecting data."%wait_time
    time.sleep(wait_time)

    sleeper = rospy.Rate(30)
    for i in xrange(n_tfm+1):
        if freq is None:
            raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm), "red", True))
        else: 
            print colorize("Transform %d of %d."%(i, n_tfm), "red", True)
        ## transforms which need to be averaged.
        ar_tfm_avgs = []
        ph_tfm_avgs = []
        
        for j in xrange(n_avg):            
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)
            
            mtrans, mrot, ptrans, prot = None, None, None, None
            while ptrans == None or mtrans == None:
                mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0))
                ptrans, prot = tf_sub.lookupTransform(ph.PHASESPACE_FRAME, ps_frame, rospy.Time(0))
                sleeper.sleep()

            ar_tfm_avgs.append(conversions.trans_rot_to_hmat(mtrans,mrot))
            ph_tfm_avgs.append(conversions.trans_rot_to_hmat(ptrans,prot))
            
        ar_tfm = utils.avg_transform(ar_tfm_avgs)
        ph_tfm = utils.avg_transform(ph_tfm_avgs)

#         print "\nar:"
#         print ar_tfm
#         print ar_tfm.dot(I_0).dot(ar_tfm.T)
#         print "h:"
#         print ph_tfm
#         print ph_tfm.dot(I_0).dot(ph_tfm.T), "\n"
                
        ar_tfms.append(ar_tfm)
        ph_tfms.append(ph_tfm)
        if freq is not None:
            time.sleep(1/freq)

        
    print "Found %i transforms. Calibrating..."%n_tfm
    Tas = ss.solve4(ar_tfms, ph_tfms)
    print "Done."
    
    T_cps = [ar_tfms[i].dot(Tas).dot(np.linalg.inv(ph_tfms[i])) for i in xrange(len(ar_tfms))]
    return utils.avg_transform(T_cps)
Ejemplo n.º 31
0
def make_shell_calls(cmd_params):
    test_command, demo_name, perturb_name = cmd_params
    shell_cmd = test_command%(args.init_state_h5, args.demo_type, demo_name, perturb_name, state_dir)
    print colorize("Calling:\n\t"+shell_cmd, "green", True)
    return os.system(shell_cmd)