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