Ejemplo n.º 1
0
def add_bag_to_hdf(bag, annotations, hdfroot, demo_name):
    joint_names, stamps, traj = extract_joints(bag)
    traj = np.asarray(traj)
    stamps = np.asarray(stamps)
    
    robot = get_robot()

    for seg_info in annotations:


        group = hdfroot.create_group(demo_name + "_" + seg_info["name"])
    
        start = seg_info["start"]
        stop = seg_info["stop"]
        
        [i_start, i_stop] = np.searchsorted(stamps, [start, stop])
        
        stamps_seg = stamps[i_start:i_stop+1]
        traj_seg = traj[i_start:i_stop+1]
         
        sample_inds = fastrapp.resample(traj_seg, [], .01)
    
        traj_ds = traj_seg[sample_inds,:]
        stamps_ds = stamps_seg[sample_inds]
    
        group["description"] = seg_info["description"]
        group["stamps"] = stamps_ds
        group.create_group("joint_states")
        group["joint_states"]["name"] = joint_names
        group["joint_states"]["position"] = traj_ds
        link_names = ["l_gripper_tool_frame","r_gripper_tool_frame"]
        special_joint_names = ["l_gripper_joint", "r_gripper_joint"]
        manip_names = ["leftarm", "rightarm"]
        
        add_kinematics_to_group(group, link_names, manip_names, special_joint_names, robot)
Ejemplo n.º 2
0
def add_bag_to_hdf(bag, annotations, hdfroot, demo_name):
    joint_names, stamps, traj = extract_joints(bag)
    traj = np.asarray(traj)
    stamps = np.asarray(stamps)
    
    robot = get_robot()

    for seg_info in annotations:


        group = hdfroot.create_group(demo_name + "_" + seg_info["name"])
    
        start = seg_info["start"]
        stop = seg_info["stop"]
        look = seg_info["look"]
        
        [i_start, i_stop, i_look] = np.searchsorted(stamps, [start, stop, look])
        
        stamps_seg = stamps[i_start:i_stop+1]
        traj_seg = traj[i_start:i_stop+1]
        look_dofs = traj[i_look]
        sample_inds = fastrapp.resample(traj_seg, np.arange(len(traj_seg)), .01, np.inf, np.inf)
        print "trajectory has length", len(sample_inds),len(traj_seg)

    
        traj_ds = traj_seg[sample_inds,:]
        stamps_ds = stamps_seg[sample_inds]
    
        group["description"] = seg_info["description"]
        group["stamps"] = stamps_ds
        group.create_group("joint_states")
        group["joint_states"]["name"] = joint_names
        group["joint_states"]["position"] = traj_ds
        group["joint_states"]["look_position"] = look_dofs
        link_names = ["l_gripper_tool_frame","r_gripper_tool_frame","l_gripper_r_finger_tip_link","l_gripper_l_finger_tip_frame","r_gripper_r_finger_tip_link","r_gripper_l_finger_tip_frame"]
        special_joint_names = ["l_gripper_joint", "r_gripper_joint"]
        manip_names = ["leftarm", "rightarm"]
        
        add_kinematics_to_group(group, link_names, manip_names, special_joint_names, robot)
        
        if seg_info.get('key_points') is not None:
            group.create_group("key_points")
            for key in seg_info["key_points"]:
                group["key_points"][key] = seg_info["key_points"][key]
        if seg_info.get("ar_marker_poses") is not None:
            group.create_group("ar_marker_poses")
            for i in seg_info["ar_marker_poses"]:
                group["ar_marker_poses"][str(i)] = seg_info["ar_marker_poses"][i]
        if seg_info.get('ree') is not None:
            group["ree"] = seg_info["ree"]
        if seg_info.get('lee') is not None:
            group["lee"] = seg_info["lee"]
        if seg_info.get('extra_information') is not None:
            group["extra_information"] = seg_info["extra_information"]
Ejemplo n.º 3
0
def test_resample():
    x = [0, 0, 0, 1, 2, 3, 4, 4, 4]
    t = range(len(x))
    inds = adaptive_resample(x, max_err=1e-5)
    assert inds == [0, 2, 6, 8]
    inds = adaptive_resample(x, t=t, max_err=0)
    assert inds == [0, 2, 6, 8]
    print "success"

    inds1 = fastrapp.resample(np.array(x)[:, None], t, 0, np.inf, np.inf)
    print inds1
    assert inds1.tolist() == [0, 2, 6, 8]
Ejemplo n.º 4
0
def test_resample():
    x = [0, 0, 0, 1, 2, 3, 4, 4, 4]
    t = range(len(x))
    inds = adaptive_resample(x, max_err=1e-5)
    assert inds == [0, 2, 6, 8]
    inds = adaptive_resample(x, t=t, max_err=0)
    assert inds == [0, 2, 6, 8]
    print "success"

    inds1 = fastrapp.resample(np.array(x)[:, None], t, 0, np.inf, np.inf)
    print inds1
    assert inds1.tolist() == [0, 2, 6, 8]
Ejemplo n.º 5
0
def test_resample_big():
    from time import time
    t = np.linspace(0,1,1000)
    x0 = np.sin(t)[:,None]
    x = x0 + np.random.randn(len(x0), 50)*.1
    tstart = time()
    inds0 = adaptive_resample(x, t=t, max_err = .05, max_dt = .1)
    print time() - tstart, "seconds"

    print "now doing cpp version"
    tstart = time()
    inds1 = fastrapp.resample(x, t, .05, np.inf, .1)
    print time() - tstart, "seconds"
    
    assert np.allclose(inds0, inds1)
Ejemplo n.º 6
0
def test_resample_big():
    from time import time
    t = np.linspace(0, 1, 1000)
    x0 = np.sin(t)[:, None]
    x = x0 + np.random.randn(len(x0), 50) * .1
    tstart = time()
    inds0 = adaptive_resample(x, t=t, max_err=.05, max_dt=.1)
    print time() - tstart, "seconds"

    print "now doing cpp version"
    tstart = time()
    inds1 = fastrapp.resample(x, t, .05, np.inf, .1)
    print time() - tstart, "seconds"

    assert np.allclose(inds0, inds1)