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:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["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, :]
r2r = ros2rave.RosToRave(Globals.robot,
asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot,
asarray(fake_seg["joint_states"]["position"][0]))
else:
Globals.pr2.head.set_pan_tilt(0, 1.2)
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
time.sleep(.5)
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.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s" % (seg_name))
if "endstates" in seg_name:
redprint("DONE!")
break
################################
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)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f, _ = registration.tps_rpm_bij(scaled_old_xyz,
scaled_new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=5 if args.animation else 0,
rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=50,
reg_init=10,
reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
handles.extend(
plotting_openrave.draw_grid(Globals.env,
f.transform_points,
old_xyz.min(axis=0) - np.r_[0, 0, .1],
old_xyz.max(axis=0) + np.r_[0, 0, .1],
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)))
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))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
1])
#print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
if arm_moved(old_joint_traj):
lr2oldtraj[lr] = old_joint_traj
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
_, timesteps_rs = unif_resample(old_total_traj,
JOINT_LENGTH_PER_STEP)
####
### Generate fullbody traj
bodypart2traj = {}
for (lr, old_joint_traj) in lr2oldtraj.items():
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj_rs = mu.interp2d(timesteps_rs,
np.arange(len(old_joint_traj)),
old_joint_traj)
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(
timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
new_joint_traj = planning.plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,
old_joint_traj_rs)
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
success &= set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if not success: break
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
if not success: break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
robot = pr2.robot
pr2.tf_listener.waitForTransform("/base_footprint",
"/camera_rgb_optical_frame", rospy.Time(0),
rospy.Duration(1))
(x, y, z), (qx, qy, qz,
qw) = pr2.tf_listener.lookupTransform("/base_footprint",
"/camera_rgb_optical_frame",
rospy.Time(0))
T_w_k = openravepy.matrixFromPose([qw, qx, qy, qz, x, y, z])
if not args.dry_run: np.savetxt(osp.join(scene_dir, "kinect_frame.txt"), T_w_k)
dof_vals = robot.GetDOFValues()
if not args.dry_run: np.savetxt(osp.join(scene_dir, "dof_vals.txt"), dof_vals)
grabber = cloudprocpy.CloudGrabber()
xyzrgb = grabber.getXYZRGB()
if not args.dry_run:
xyzrgb.save(osp.join(scene_dir, "cloud.pcd"))
#clouds = []
#grabber.startXYZ()
#for i in xrange(10):
#xyzrgb = grabber.getXYZ()
#clouds.append(xyzrgb.to3dArray())
#grabber.stop()
#avg_cloud = cloudprocpy.CloudXYZ()
#avg_cloud.from3dArray(bottleneck.nanmean(np.array(clouds), axis=0))
#if not args.dry_run: avg_cloud.save(osp.join(scene_dir,"cloud.pcd"))
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