def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0,_ = best_demo["cloud_xyz_ds"]
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1, plotting = 4, reg_init=1,reg_final=args.reg_final,n_iter=40)
else:
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False)#, interactive=True)
np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame"%lr]["position"],
warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True
)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm"%lr] = arm_traj
trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm"%lr],
Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr
)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST,
ns='warped_finger_traj'
))
# plot original trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST,
ns='demo_finger_traj'
))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
visualization:
- show all library states
- warping visualization from matlab
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled(xyz_new,xyz_new_ds, ds_inds)
if HUMAN_SELECT_DEMO:
seg_name = trajectory_library.interactive_select_demo(self.library)
best_demo = self.library.root[seg_name]
pts0,_ = downsample(np.asarray(best_demo["cloud_xyz"]))
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1,
plotting = 4, reg_init=1,reg_final=.025,n_iter=40)
else:
best_f = None
best_cost = np.inf
best_name = None
for (seg_name,candidate_demo) in self.library.root.items():
xyz_demo = np.squeeze(np.asarray(candidate_demo["cloud_xyz"]))
if args.obj == "cloth": xyz_demo = voxel_downsample(xyz_demo, .025)
xyz_demo_ds, ds_inds = downsample(xyz_demo)#voxel_downsample(xyz_demo, DS_LENGTH, return_inds = True)
dists_demo = recognition.calc_geodesic_distances_downsampled(xyz_demo, xyz_demo_ds, ds_inds)
cost = recognition.calc_match_score(xyz_new_ds, xyz_demo_ds, dists0 = dists_new, dists1 = dists_demo)
print "seg_name: %s. cost: %s"%(seg_name, cost)
if cost < best_cost:
best_cost = cost
best_name = seg_name
#if best_name.startswith("done"): return "done"
best_demo = self.library.root[best_name]
xyz_demo_ds,_ = downsample(np.asarray(best_demo["cloud_xyz"][0]))
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds,
plotting = 10, reg_init=1,reg_final=.01,n_iter=200,verbose=True)
print "best segment:", best_name
orig_pose_array = conversions.array_to_pose_array(best_demo["cloud_xyz"][0], "base_footprint")
warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(best_demo["cloud_xyz"][0]), "base_footprint")
HANDLES.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
HANDLES.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
mins = best_demo["cloud_xyz"][0].min(axis=0)
maxes = best_demo["cloud_xyz"][0].max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
HANDLES.append(grid_handle)
#self.f = fit_tps(demo["rope"][0], userdata.points)
userdata.left_used = left_used = best_demo["arms_used"].value in "lb"
userdata.right_used = right_used = best_demo["arms_used"].value in "rb"
print "left_used", left_used
print "right_used", right_used
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo, left=left_used, right=right_used)
trajectory = np.zeros(len(best_demo["times"]), dtype=trajectories.BodyState)
Globals.pr2.update_rave()
if left_used:
l_arm_traj, feas_inds = trajectories.make_joint_traj(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"], Globals.pr2.robot.GetManipulator("leftarm"),"base_footprint","l_gripper_tool_frame",1+16)
if len(feas_inds) == 0: return "failure"
trajectory["l_arm"] = l_arm_traj
rospy.loginfo("left arm: %i of %i points feasible", len(feas_inds), len(trajectory))
trajectory["l_gripper"] = fix_gripper(warped_demo["l_gripper_angles"])
HANDLES.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]),
"base_footprint"),
width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
if right_used:
r_arm_traj,feas_inds = trajectories.make_joint_traj(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"], Globals.pr2.robot.GetManipulator("rightarm"),"base_footprint","r_gripper_tool_frame",1+16)
if len(feas_inds) == 0: return "failure"
trajectory["r_arm"] = r_arm_traj
rospy.loginfo("right arm: %i of %i points feasible", len(feas_inds), len(trajectory))
trajectory["r_gripper"] = fix_gripper(warped_demo["r_gripper_angles"])
HANDLES.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]),
"base_footprint"),
width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
userdata.trajectory = trajectory
#userdata.base_xya = (x,y,0)
# todo: draw pr2
# consent = yes_or_no("trajectory ok?")
consent = True
if consent: return "not_done"
else: return "failure"
from lfd import trajectory_library as tl
library = tl.TrajectoryLibrary("lm.h5", "read")
tl.interactive_select_demo(library)
from lfd import trajectory_library as tl
library = tl.TrajectoryLibrary("lm.h5", "read")
tl.interactive_select_demo(library)
def execute(self, userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(
xyz_new, xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0, _ = best_demo["cloud_xyz_ds"]
pts1, _ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0,
pts1,
plotting=4,
reg_init=1,
reg_final=args.reg_final,
n_iter=40)
else:
if args.count_steps:
candidate_demo_names = self.count2segnames[Globals.stage]
else:
candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(
parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds,
dists_new)
for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds,
xyz_new_ds,
plotting=20,
reg_init=1,
reg_final=.01,
n_iter=n_iter,
verbose=False) #, interactive=True)
np.savez('registration_data',
xyz_demo_ds=xyz_demo_ds,
xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points,
mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(
np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(
warped_demo["l_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
clipinplace(
warped_demo["r_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame" % lr]["position"],
warped_demo["%s_gripper_tool_frame" % lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr,
check_collisions=True)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm" % lr] = arm_traj
trajectory["%s_grab" %
lr] = best_demo["%s_gripper_joint" % lr] < .07
trajectory["%s_gripper" %
lr] = warped_demo["%s_gripper_joint" % lr]
trajectory["%s_gripper" % lr][trajectory["%s_grab" % lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory[
"%s_arm" %
lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm" % lr], Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm" % other_lr] = lfd_traj.fill_stationary(
trajectory["%s_arm" % other_lr], discont_times,
n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_grab" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_gripper" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr][trajectory["%s_grab" % tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
warped_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
warped_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(1, 0, 1, .4),
type=Marker.LINE_LIST,
ns='warped_finger_traj'))
# plot original trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
best_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
best_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(0, 1, 1, .4),
type=Marker.LINE_LIST,
ns='demo_finger_traj'))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
