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)
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"]
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]
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]
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)
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)
