def plot_original_and_warped_demo_and_spec_pt(best_demo, warped_demo, spec_pt_traj, warped_spec_pt_traj, traj):
arms_used = best_demo["arms_used"]
if arms_used in "lb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["l_gripper_tool_frame"]["position"]), 'base_footprint', asarray(best_demo["l_gripper_tool_frame"]["orientation"]))
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["l_gripper_tool_frame"]["position"]), 'base_footprint', asarray(warped_demo["l_gripper_tool_frame"]["orientation"]))
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.l_gripper_poses, traj.l_gripper_angles, ns = "make_verb_traj_service_grippers"))
if arms_used in "rb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["r_gripper_tool_frame"]["position"]), 'base_footprint', asarray(best_demo["r_gripper_tool_frame"]["orientation"]))
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["r_gripper_tool_frame"]["position"]), 'base_footprint', asarray(warped_demo["r_gripper_tool_frame"]["orientation"]))
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.r_gripper_poses, traj.r_gripper_angles, ns = "make_verb_traj_service_grippers"))
for (clouds,rgba) in [(sorted_values(best_demo["object_clouds"]),(1,0,0,.5)),
(sorted_values(warped_demo["object_clouds"]),(0,1,0,.5))]:
cloud = []
for subcloud in clouds:
cloud.extend(np.asarray(subcloud["xyz"]).reshape(-1,3))
cloud = np.array(cloud)
cloud = voxel_downsample(cloud, .02)
pose_array = conversions.array_to_pose_array(cloud, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = rgba,width=.01,type=Marker.CUBE_LIST))
def plot_orig_and_warped_clouds(f, x_nd, y_md, res=.05, d=3, ns_prefix='tpsrpm', force_pyplot=False, ax=None):
if ax is None: ax = plt
if d==2:
ax.plot(x_nd[:,1], x_nd[:,0],'r.')
ax.plot(y_md[:,1], y_md[:,0], 'b.')
pred = f(x_nd)
ax.plot(pred[:,1], pred[:,0], 'g.')
plot_warped_grid_2d(f, x_nd.min(axis=0), x_nd.max(axis=0))
ax.ginput()
elif d == 3:
if force_pyplot:
mins = np.r_[x_nd, y_md].min(axis=0) - np.array([.5, .5, .01])
maxes = np.r_[x_nd, y_md].max(axis=0) + np.array([.5, .5, .01])
#warping.draw_grid_pyplot(f, mins[:2], maxes[:2], grid_res=res)
warping.draw_grid_pyplot(ax, f, mins, maxes, xres=res, yres=res, zres=None)
ax.scatter(x_nd[:,0], x_nd[:,1], color=(1, 0, 0), label='demo')
ax.scatter(y_md[:,0], y_md[:,1], color=(0, 0, 1), label='test')
fx_nd = f(x_nd)
ax.scatter(fx_nd[:,0], fx_nd[:,1], color='brown', alpha=.5, label='warped demo')
else:
Globals.setup()
mins = x_nd.min(axis=0)
maxes = x_nd.max(axis=0)
mins -= np.array([.1, .1, .01])
maxes += np.array([.1, .1, .01])
Globals.handles = warping.draw_grid(Globals.rviz, f, mins, maxes, 'base_footprint', xres=res, yres=res, zres=-1, ns=ns_prefix+'_grid')
orig_pose_array = conversions.array_to_pose_array(x_nd, "base_footprint")
target_pose_array = conversions.array_to_pose_array(y_md, "base_footprint")
warped_pose_array = conversions.array_to_pose_array(f(x_nd), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,.9),type=Marker.CUBE_LIST, ns=ns_prefix+'_demo_cloud'))
Globals.handles.append(Globals.rviz.draw_curve(target_pose_array,rgba=(0,0,1,.9),type=Marker.CUBE_LIST, ns=ns_prefix+'_target_cloud'))
Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,.9),type=Marker.CUBE_LIST, ns=ns_prefix+'_warped_cloud'))
def draw_movement_markers(rope, r, rgba, ns):
pts = rope[::(len(rope)/5 or 1)]
angles = np.random.rand(pts.shape[0])*2*np.pi
pts2 = pts.copy()
pts2[:,:2] += np.c_[r*np.cos(angles), r*np.sin(angles)]
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(alternate(pts, pts2), "base_footprint"),
rgba=rgba, type=Marker.LINE_LIST, ns=ns
))
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(pts2, "base_footprint"),
rgba=rgba, type=Marker.CUBE_LIST, ns=ns, width=.02
))
def tps_rpm(x_nd, y_md, n_iter = 5, reg_init = .1, reg_final = .001, rad_init = .2, rad_final = .001, plotting = False, verbose=True, f_init = None, return_full = False, ns_prefix='tpsrpm', p=.2, interactive=False, show_corr=False):
"""
tps-rpm algorithm mostly as described by chui and rangaran
reg_init/reg_final: regularization on curvature
rad_init/rad_final: radius for correspondence calculation (meters)
plotting: 0 means don't plot. integer n means plot every n iterations
"""
n,d = x_nd.shape
regs = loglinspace(reg_init, reg_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
f = ThinPlateSpline.identity(d)
#f.trans_g = y_md.mean(axis=0) - x_nd.mean(axis=0)
for i in xrange(n_iter):
if f.d==2 and i%plotting==0:
import matplotlib.pyplot as plt
plt.clf()
if i==0 and f_init is not None:
xwarped_nd = f_init(x_nd)
print xwarped_nd.max(axis=0)
else:
xwarped_nd = f.transform_points(x_nd)
if plotting and i%plotting == 0 and interactive:
raw_input('press enter to continue')
# targ_nd = find_targets(x_nd, y_md, corr_opts = dict(r = rads[i], p = .1))
corr_nm = calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=p)
wt_n = corr_nm.sum(axis=1)
#goodn = wt_n > .1
goodn = wt_n > -.1
targ_Nd = np.dot(corr_nm[goodn, :]/wt_n[goodn][:,None], y_md)
assert (corr_nm[goodn,:].sum(axis=1) == wt_n[goodn]).all()
#print "warning: changed angular spring!"
f.fit(x_nd[goodn], targ_Nd, bend_coef = regs[i], wt_n = wt_n[goodn], rot_coef = 10*regs[i], verbose=verbose)
if plotting and i%plotting==0:
plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md, ns_prefix=ns_prefix)
targ_pose_array = conversions.array_to_pose_array(targ_Nd, 'base_footprint')
#Globals.handles.append(Globals.rviz.draw_curve(targ_pose_array,rgba=(1,1,0,1),type=Marker.CUBE_LIST, ns=ns_prefix+'_targ_Nd'))
if show_corr:
plot_correspondence_3d(x_nd[goodn], targ_Nd)
if return_full:
info = {}
info["corr_nm"] = corr_nm
info["goodn"] = goodn
info["x_Nd"] = x_nd[goodn,:]
info["targ_Nd"] = targ_Nd
info["wt_N"] = wt_n[goodn]
return f, info
else:
return f
def draw_grid(rviz,
f,
mins,
maxes,
frame_id,
xres=.1,
yres=.1,
zres=.04,
ns='default_ns'):
grid_handles = []
xmin, ymin, zmin = mins
xmax, ymax, zmax = maxes
ncoarse = 10
nfine = 30
xcoarse = np.arange(xmin, xmax, xres)
ycoarse = np.arange(ymin, ymax, yres)
if zres == -1: zcoarse = [(zmin + zmax) / 2.]
else: zcoarse = np.arange(zmin, zmax, zres)
xfine = np.linspace(xmin, xmax, nfine)
yfine = np.linspace(ymin, ymax, nfine)
zfine = np.linspace(zmin, zmax, nfine)
lines = []
if len(zcoarse) > 1:
for x in xcoarse:
for y in ycoarse:
xyz = np.zeros((nfine, 3))
xyz[:, 0] = x
xyz[:, 1] = y
xyz[:, 2] = zfine
lines.append(f(xyz))
for y in ycoarse:
for z in zcoarse:
xyz = np.zeros((nfine, 3))
xyz[:, 0] = xfine
xyz[:, 1] = y
xyz[:, 2] = z
lines.append(f(xyz))
for z in zcoarse:
for x in xcoarse:
xyz = np.zeros((nfine, 3))
xyz[:, 0] = x
xyz[:, 1] = yfine
xyz[:, 2] = z
lines.append(f(xyz))
for line in lines:
grid_handles.append(
rviz.draw_curve(conversions.array_to_pose_array(line, frame_id),
width=.0005,
rgba=(1, 1, 0, 1),
ns=ns))
return grid_handles
def default_plot_callback(x_nd, y_md, targ_nd, corr_nm, wt_n, f):
del Globals.handles[:]
plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md)
targ_pose_array = conversions.array_to_pose_array(targ_nd,
'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(targ_pose_array,
rgba=(1, 1, 0, 1),
type=Marker.CUBE_LIST))
def filter_pc2(cloud_pc2):
cloud_xyz = (pc2xyzrgb(cloud_pc2)[0]).reshape(-1,3)
cloud_xyz_down = voxel_downsample(cloud_xyz, .02)
graph = ri.points_to_graph(cloud_xyz_down, .03)
cc = ri.largest_connected_component(graph)
good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()])
pose_array = juc.array_to_pose_array(good_xyzs, "base_footprint")
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation"))
raw_input("press enter when done looking")
del Globals.handles[:]
return xyz2pc(good_xyzs, cloud_pc2.header.frame_id)
def draw_grid(rviz, f, mins, maxes, frame_id, xres = .1, yres = .1, zres = .04, ns='default_ns'):
grid_handles = []
xmin, ymin, zmin = mins
xmax, ymax, zmax = maxes
ncoarse = 10
nfine = 30
xcoarse = np.arange(xmin, xmax, xres)
ycoarse = np.arange(ymin, ymax, yres)
if zres == -1: zcoarse = [(zmin+zmax)/2.]
else: zcoarse = np.arange(zmin, zmax, zres)
xfine = np.linspace(xmin, xmax, nfine)
yfine = np.linspace(ymin, ymax, nfine)
zfine = np.linspace(zmin, zmax, nfine)
lines = []
if len(zcoarse) > 1:
for x in xcoarse:
for y in ycoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = x
xyz[:,1] = y
xyz[:,2] = zfine
lines.append(f(xyz))
for y in ycoarse:
for z in zcoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = xfine
xyz[:,1] = y
xyz[:,2] = z
lines.append(f(xyz))
for z in zcoarse:
for x in xcoarse:
xyz = np.zeros((nfine, 3))
xyz[:,0] = x
xyz[:,1] = yfine
xyz[:,2] = z
lines.append(f(xyz))
for line in lines:
grid_handles.append(rviz.draw_curve(conversions.array_to_pose_array(line, frame_id),width=.0005,rgba=(1,1,0,1), ns=ns))
return grid_handles
def draw_rope(rope, width, rgba, ns):
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(rope, "base_footprint"),
width=width, rgba=rgba, type=Marker.LINE_STRIP, ns=ns
))
def select_trajectory(points, curr_robot_joint_vals, curr_step):
"""
- 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(points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
# xyz_new_ds, ds_inds = xyz_new.reshape(-1,3), np.arange(0, len(xyz_new)).reshape(-1, 1)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
candidate_demo_names = Globals.demos.keys()
#from joblib import parallel
#costs_names = parallel.Parallel(n_jobs = 4)(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 sorted(candidate_demo_names)]
_, best_name = min(costs_names)
print "choices: ", candidate_demo_names
best_name = None
while best_name not in Globals.demos:
best_name = raw_input("type name of trajectory you want to use\n")
rospy.loginfo('costs_names %s', costs_names)
#matcher = recognition.CombinedNNMatcher(recognition.DataSet.LoadFromDict(Globals.demos), [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1])
#best_name, best_cost = matcher.match(xyz_new)
best_demo = Globals.demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return {'status': 'success'}
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
# print 'arms used', best_demo['arms_used']
# overlap_ctl_pts = []
# grabbing_pts = []
# for lr in 'lr':
# # look at points around gripper when grabbing
# grabbing = map(bool, list(best_demo["%s_gripper_joint"%lr] < .07))
# grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])])
# grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])])
# overlap_ctl_pts = [p for p in xyz_demo_ds if any(np.linalg.norm(p - g) < 0.1 for g in grabbing_pts)]
# overlap_ctl_pts = xyz_demo_ds
#rviz_draw_points(overlap_ctl_pts,rgba=(1,1,1,1),type=Marker.CUBE_LIST)
# rviz_draw_points(grabbing_pts,rgba=(.5,.5,.5,1),type=Marker.CUBE_LIST)
n_iter = 101
#warping_map = registration.tps_rpm_with_overlap_control(xyz_demo_ds, xyz_new_ds, overlap_ctl_pts, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20)
warping_map,info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20,return_full=True)
from lfd import tps
import scipy.spatial.distance as ssd
f = warping_map
pts_grip = []
for lr in "lr":
if best_demo["arms_used"] in ["b", lr]:
pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"])
pts_grip = np.array(pts_grip)
dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1)
pts_grip_near_rope = pts_grip[dist_to_rope < .04,:]
pts_rigid = voxel_downsample(pts_grip_near_rope, .01)
Globals.handles = []
registration.Globals.handles = []
f.lin_ag, f.trans_g, f.w_ng, f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5)
#if plotting:
#plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md)
#targ_pose_array = conversions.array_to_pose_array(targ_Nd, 'base_footprint')
#Globals.handles.append(Globals.rviz.draw_curve(targ_pose_array,rgba=(1,1,0,1),type=Marker.CUBE_LIST))
#raw_input('Press enter to continue:')
#################### 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(warping_map.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, ns='demo_cloud'))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST, ns='warped_cloud'))
#### 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, warping_map.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(warping_map, best_demo)
Globals.pr2.update_rave_without_ros(curr_robot_joint_vals)
trajectory = {}
trajectory['seg_name'] = best_name
trajectory['demo'] = best_demo
if 'tracked_states' in best_demo:
trajectory['orig_tracked_states'] = best_demo['tracked_states']
trajectory['tracked_states'], Globals.offset_trans = warping.transform_tracked_states(warping_map, best_demo, Globals.offset_trans)
steps = 0
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)
rospy.loginfo("calculating joint trajectory...")
#arm_traj, feas_inds = lfd_traj.make_joint_traj(
# warped_demo["%s_gripper_tool_frame"%lr]["position"],
# warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
# best_demo["%sarm"%leftright],
# Globals.pr2.robot.GetManipulator("%sarm"%leftright),
# "base_footprint","%s_gripper_tool_frame"%lr,
# 1+2+16
#)
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 {'status': "failure"}
trajectory["%s_arm"%lr] = arm_traj
trajectory["%s_steps"%lr] = steps = len(arm_traj)
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(arm_traj))
trajectory["%s_grab"%lr] = map(bool, list(best_demo["%s_gripper_joint"%lr] < .02))
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# 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'
))
assert 'l_steps' not in trajectory or steps == trajectory['l_steps']
assert 'r_steps' not in trajectory or steps == trajectory['r_steps']
trajectory['steps'] = steps
#raw_input('Press enter to continue:')
return {'status': 'not_done', 'trajectory': trajectory}
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
if (args.plotting):
pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id)
Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")]
return resp
if __name__ == "__main__":
if args.test:
if rospy.get_name() == "/unnamed":
rospy.init_node("test_interactive_segmentation_service",disable_signals=True)
listener = tf.TransformListener()
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id)
Globals.setup()
req = ProcessCloudRequest()
req.cloud_in = pc_tf
resp = callback(req)
xyz, rgb = pc2xyzrgb(resp.cloud_out)
pose_array = conversions.array_to_pose_array(xyz.reshape(-1,3), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST,width=.002, ns = 'segmentation'))
else:
rospy.init_node("test_interactive_segmentation_service",disable_signals=True)
Globals.setup()
service = rospy.Service("interactive_segmentation", ProcessCloud, callback)
time.sleep(1000000)
def draw_pc(xyzs, rgba):
pose_array = conversions.array_to_pose_array(xyzs, "base_footprint")
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba=rgba, type=Marker.CUBE_LIST))
def warping_loop(args, take_snapshot, quit):
if rospy.get_name() == '/unnamed':
rospy.init_node('publish_single_cloud', disable_signals=True)
Globals.setup()
demo = load_demo(args.taskname, args.demos_list_file, args.seg_name)
imarker_server = None
if args.warp_type == 'rigid_interactive':
from interactive_markers.interactive_marker_server import *
init_pos = demo['cloud_xyz'].mean(axis=0)[:3]
marker_6dof = utils_rviz.make_interactive_marker_6dof(init_pos, args.frame)
imarker_server = InteractiveMarkerServer('demo_pose_control')
def callback_fn(feedback):
xyz = feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z
imarker_server.insert(marker_6dof, callback_fn)
imarker_server.applyChanges()
# align demo to test in a loop
#prev_params = None
keep_warping = True
num_saved = 0
while not quit.value:
xyz_new = load_cloud_from_sensor(args.input_topic, args.frame)
if xyz_new.size == 0: continue
xyz_demo_ds = demo['cloud_xyz_ds']
xyz_new_ds = recognition.downsample(xyz_new)[0]
if keep_warping:
f = fit_warp(args, xyz_demo_ds, xyz_new_ds, set_warp_params=args.set_warp_params)
if hasattr(f, 'get_params') and args.set_warp_params is None:
print 'fitted warp params: "%s"' % (' '.join(map(str, f.get_params())), )
if args.warp_once_only:
keep_warping = False
if args.show_shape_contexts:
shape_context_costs = recognition.match_and_calc_shape_context(xyz_demo_ds, xyz_new_ds, normalize_costs=True)
colors = [(c, 1-c, 0, 1) for c in shape_context_costs]
else:
import itertools
colors = itertools.repeat((1, 0, 0, 1))
warped_pose_array = conversions.array_to_pose_array(f.transform_points(xyz_demo_ds), args.frame)
Globals.handles = []
import geometry_msgs.msg as gm
for p, rgba in zip(warped_pose_array.poses, colors):
ps = gm.PoseStamped()
ps.header = warped_pose_array.header
ps.pose = p
Globals.handles.append(Globals.rviz.draw_marker(ps, scale=(.01, .01, .01), rgba=rgba))
if take_snapshot.value:
filename = '%s%04d' % (args.save_prefix, num_saved)
print 'Saving snapshot to %s' % (filename,)
take_snapshot.value = False
finv = fit_warp(args, xyz_new_ds, xyz_demo_ds, set_warp_params=args.set_inv_warp_params)
xyz_new_ds_out = finv.transform_points(xyz_new_ds)
xyz_new_out = finv.transform_points(xyz_new)
np.savez(filename, cloud_xyz=xyz_new_out, cloud_xyz_ds=xyz_new_ds_out)
num_saved += 1
print 'Done saving snapshot. (used inverse transformation "%s")' % (' '.join(map(str, finv.get_params())), )
def plot_traj(xyzs, rgba, quats=None):
pose_array = juc.array_to_pose_array(np.asarray(xyzs), 'base_footprint', quats)
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = rgba, ns = "multi_item_make_verb_traj_service"))
def warping_loop(args, take_snapshot, quit):
if rospy.get_name() == '/unnamed':
rospy.init_node('publish_single_cloud', disable_signals=True)
Globals.setup()
demo = load_demo(args.taskname, args.demos_list_file, args.seg_name)
imarker_server = None
if args.warp_type == 'rigid_interactive':
from interactive_markers.interactive_marker_server import *
init_pos = demo['cloud_xyz'].mean(axis=0)[:3]
marker_6dof = utils_rviz.make_interactive_marker_6dof(
init_pos, args.frame)
imarker_server = InteractiveMarkerServer('demo_pose_control')
def callback_fn(feedback):
xyz = feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z
imarker_server.insert(marker_6dof, callback_fn)
imarker_server.applyChanges()
# align demo to test in a loop
#prev_params = None
keep_warping = True
num_saved = 0
while not quit.value:
xyz_new = load_cloud_from_sensor(args.input_topic, args.frame)
if xyz_new.size == 0: continue
xyz_demo_ds = demo['cloud_xyz_ds']
xyz_new_ds = recognition.downsample(xyz_new)[0]
if keep_warping:
f = fit_warp(args,
xyz_demo_ds,
xyz_new_ds,
set_warp_params=args.set_warp_params)
if hasattr(f, 'get_params') and args.set_warp_params is None:
print 'fitted warp params: "%s"' % (' '.join(
map(str, f.get_params())), )
if args.warp_once_only:
keep_warping = False
if args.show_shape_contexts:
shape_context_costs = recognition.match_and_calc_shape_context(
xyz_demo_ds, xyz_new_ds, normalize_costs=True)
colors = [(c, 1 - c, 0, 1) for c in shape_context_costs]
else:
import itertools
colors = itertools.repeat((1, 0, 0, 1))
warped_pose_array = conversions.array_to_pose_array(
f.transform_points(xyz_demo_ds), args.frame)
Globals.handles = []
import geometry_msgs.msg as gm
for p, rgba in zip(warped_pose_array.poses, colors):
ps = gm.PoseStamped()
ps.header = warped_pose_array.header
ps.pose = p
Globals.handles.append(
Globals.rviz.draw_marker(ps, scale=(.01, .01, .01), rgba=rgba))
if take_snapshot.value:
filename = '%s%04d' % (args.save_prefix, num_saved)
print 'Saving snapshot to %s' % (filename, )
take_snapshot.value = False
finv = fit_warp(args,
xyz_new_ds,
xyz_demo_ds,
set_warp_params=args.set_inv_warp_params)
xyz_new_ds_out = finv.transform_points(xyz_new_ds)
xyz_new_out = finv.transform_points(xyz_new)
np.savez(filename,
cloud_xyz=xyz_new_out,
cloud_xyz_ds=xyz_new_ds_out)
num_saved += 1
print 'Done saving snapshot. (used inverse transformation "%s")' % (
' '.join(map(str, finv.get_params())), )
def plot_spec_pts(xyzs, rgba):
pose_array = juc.array_to_pose_array(np.asarray(xyzs), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = rgba, ns = "multi_item_make_verb_traj_service"))
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"
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
xyz = xyz.reshape(-1, 3)
rgb = rgb.reshape(-1, 3)
if args.do_filtering:
xyz_down = voxel_downsample(xyz, .02)
graph = ri.points_to_graph(xyz_down, .03)
cc = ri.largest_connected_component(graph)
good_xyzs = np.array(
[graph.node[node_id]["xyz"] for node_id in cc.nodes()])
good_inds = get_good_inds(xyz, good_xyzs)
good_rgbs = rgb[good_inds]
else:
good_xyzs, good_rgbs = xyz, rgb
outfile.create_group(object_name)
outfile[object_name]["xyz"] = good_xyzs
#outfile[object_name]["rgb"] = good_rgbs
if args.plotting:
rviz = ros_utils.RvizWrapper.create()
rospy.sleep(.5)
from brett2.ros_utils import Marker
pose_array = conversions.array_to_pose_array(good_xyzs.reshape(-1, 3),
"base_footprint")
plot_handle = rviz.draw_curve(pose_array,
rgba=(1, 1, 0, 1),
type=Marker.CUBE_LIST,
width=.001,
ns="segmentation")
raw_input("press enter when done looking")
def callback(req):
xyz, rgb = pc2xyzrgb(req.cloud_in)
rgb = rgb.copy()
gc = GetClick()
cv2.setMouseCallback("rgb", gc.callback)
while not gc.done:
cv2.imshow("rgb", rgb)
cv2.waitKey(10)
gm = GetMouse()
xy_corner1 = np.array(gc.xy)
cv2.setMouseCallback("rgb", gm.callback)
while not gm.done:
img = rgb.copy()
if gm.xy is not None:
xy_corner2 = np.array(gm.xy)
cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0))
cv2.imshow("rgb",img)
cv2.waitKey(10)
xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)
#mask = np.zeros(xyz.shape[:2],dtype='uint8')
#rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy]))
#tmp1 = np.zeros((1, 13 * 5))
#tmp2 = np.zeros((1, 13 * 5))
#result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT)
#from cv2 import cv
#mask[mask == cv.GC_BGD] = 0
#mask[mask == cv.GC_PR_BGD] = 63
#mask[mask == cv.GC_FGD] = 255
#mask[mask == cv.GC_PR_FGD] = 192
#cv2.imshow('mask',mask)
#cv2.waitKey(10)
#table_height = get_table_height(xyz)
if args.grabcut:
rospy.loginfo("using grabcut")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask.fill(cv2.GC_PR_BGD)
if args.init == "height":
initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50)
mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD
elif args.init == "middle":
mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD
tmp1 = np.zeros((1, 13 * 5))
tmp2 = np.zeros((1, 13 * 5))
cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK)
else:
rospy.loginfo("using table height")
table_height = rospy.get_param("table_height")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask[np.isfinite(xyz[yl:yl+h, xl:xl+w,2]) &
(xyz[yl:yl+h, xl:xl+w,2] > table_height+.02)] = 1
mask = mask % 2
if (args.erode > 0):
mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8')
contours = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0]
cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0),thickness=2)
cv2.imshow('rgb', rgb)
cv2.waitKey(10)
zsel = xyz[yl:yl+h, xl:xl+w, 2]
mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
resp = ProcessCloudResponse()
xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
if (args.plotting):
pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id)
Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")]
return resp
def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0],.02)
y_md = voxel_downsample(new_object_clouds[0],.02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented"%transform_type)
l_offset,r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST))
return resp
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)
ELOG.log('SelectTrajectory', 'xyz_new', xyz_new)
ELOG.log('SelectTrajectory', 'xyz_new_ds', xyz_new_ds)
ELOG.log('SelectTrajectory', 'dists_new', dists_new)
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
global last_selected_segment
print 'Last selected segment:', last_selected_segment
if args.human_select_demo:
best_name = None
while best_name not in demos:
print 'Select demo from', demos.keys()
best_name = raw_input('> ')
else:
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]
ELOG.log('SelectTrajectory', 'costs_names', costs_names)
_, best_name = min(costs_names)
ELOG.log('SelectTrajectory', 'best_name', best_name)
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)
last_selected_segment = best_name
xyz_demo_ds = best_demo["cloud_xyz_ds"]
ELOG.log('SelectTrajectory', 'xyz_demo_ds', xyz_demo_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)
ELOG.log('SelectTrajectory', 'f', self.f)
else:
self.f, info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, return_full=True)#, interactive=True)
ELOG.log('SelectTrajectory', 'f', self.f)
ELOG.log('SelectTrajectory', 'f_info', info)
if args.use_nr:
rospy.loginfo('Using nonrigidity costs')
from lfd import tps
import scipy.spatial.distance as ssd
pts_grip = []
for lr in "lr":
if best_demo["arms_used"] in ["b", lr]:
pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"])
pts_grip = np.array(pts_grip)
dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1)
pts_grip_near_rope = pts_grip[dist_to_rope < .04,:]
pts_rigid = voxel_downsample(pts_grip_near_rope, .01)
self.f.lin_ag, self.f.trans_g, self.f.w_ng, self.f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5)
# 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
ELOG.log('SelectTrajectory', 'warped_demo', warped_demo)
def make_traj(warped_demo, inds=None, xyz_offset=0, feas_check_only=False):
traj = {}
total_feas_inds = 0
total_inds = 0
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)
pos = warped_demo["%s_gripper_tool_frame"%lr]["position"] + xyz_offset
ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"]
if inds is not None:
pos, ori = pos[inds], ori[inds]
if feas_check_only:
feas_inds = lfd_traj.compute_feas_inds(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm_feas_inds"%lr] = feas_inds
else:
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm"%lr] = arm_traj
traj["%s_arm_feas_inds"%lr] = feas_inds
total_feas_inds += len(feas_inds)
total_inds += len(pos)
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(pos))
return traj, total_feas_inds, total_inds
# Check if we need to move the base for reachability
base_offset = np.array([0, 0, 0])
if args.use_base:
# First figure out how much we need to move the base to maximize feasible points
OFFSET = 0.1
XYZ_OFFSETS = np.array([[0., 0., 0.], [-OFFSET, 0, 0], [OFFSET, 0, 0], [0, -OFFSET, 0], [0, OFFSET, 0]])
inds_to_check = lfd_traj.where_near_rope(best_demo, xyz_demo_ds, add_other_points=30)
need_to_move_base = False
best_feas_inds, best_xyz_offset = -1, None
for xyz_offset in XYZ_OFFSETS:
_, n_feas_inds, n_total_inds = make_traj(warped_demo, inds=inds_to_check, xyz_offset=xyz_offset, feas_check_only=True)
rospy.loginfo('Cloud offset %s has feas inds %d', str(xyz_offset), n_feas_inds)
if n_feas_inds > best_feas_inds:
best_feas_inds, best_xyz_offset = n_feas_inds, xyz_offset
if n_feas_inds >= 0.99*n_total_inds: break
if np.linalg.norm(best_xyz_offset) > 0.01:
need_to_move_base = True
base_offset = -best_xyz_offset
rospy.loginfo('Best base offset: %s, with %d feas inds', str(base_offset), best_feas_inds)
# Move the base
if need_to_move_base:
rospy.loginfo('Will move base.')
userdata.base_offset = base_offset
return 'move_base'
else:
rospy.loginfo('Will not move base.')
Globals.pr2.update_rave()
# calculate joint trajectory using IK
trajectory = make_traj(warped_demo)[0]
# fill in gripper/grab stuff
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if len(trajectory["%s_arm_feas_inds"%lr]) == 0: return "failure"
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,
ignore_inds=[1] # ignore the 0--1 discontinuity, which is usually just moving from rest to the traj starting pose
)
# 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'
))
ELOG.log('SelectTrajectory', 'trajectory', trajectory)
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"
rospy.init_node("playback_demo")
rviz = RvizWrapper.create()
pr2 = PR2.create()
rospy.sleep(1)
traj = h5py.File(args.file, "r")[args.group]
ps = gm.PoseStamped(
pose=gm.Pose(position=gm.Point(*traj["object_pose"]), orientation=gm.Quaternion(*traj["object_orientation"]))
)
ps.header.frame_id = "base_link"
rviz.draw_marker(ps, id=1, type=Marker.CUBE, rgba=(0, 1, 0, 1), scale=asarray(traj["object_dimension"]))
pose_array = conversions.array_to_pose_array(asarray(traj["gripper_positions"]), "base_link")
rviz.draw_curve(pose_array, id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"], traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat, n_waypoints, weights=np.ones(7), tol=0.001)
times = np.linspace(0, 10, n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions, _ = trajectories.make_joint_traj(
xyzquat_rs[:, 0:3], xyzquat_rs[:, 3:7], pr2.rarm.manip, "base_link", "r_wrist_roll_link", filter_options=18
)
joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=0.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
def plot_spec_pts(xyzs, rgba):
pose_array = juc.array_to_pose_array(np.asarray(xyzs), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_axes(pose_array, rgba = rgba, ns = "multi_item_make_verb_traj_service"))
pr2 = PR2.create()
rospy.sleep(1)
traj = h5py.File(args.file, 'r')[args.group]
ps = gm.PoseStamped(pose=gm.Pose(position=gm.Point(*traj["object_pose"]),
orientation=gm.Quaternion(
*traj["object_orientation"])))
ps.header.frame_id = 'base_link'
rviz.draw_marker(ps,
id=1,
type=Marker.CUBE,
rgba=(0, 1, 0, 1),
scale=asarray(traj["object_dimension"]))
pose_array = conversions.array_to_pose_array(
asarray(traj["gripper_positions"]), 'base_link')
rviz.draw_curve(pose_array, id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"], traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat,
n_waypoints,
weights=np.ones(7),
tol=.001)
times = np.linspace(0, 10, n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions, _ = trajectories.make_joint_traj(xyzquat_rs[:, 0:3],
xyzquat_rs[:, 3:7],
def draw_pc(xyzs, rgba):
pose_array = conversions.array_to_pose_array(xyzs, "base_footprint")
Globals.handles.append(
Globals.rviz.draw_curve(pose_array, rgba=rgba, type=Marker.CUBE_LIST))
def plot_original_and_warped_demo(best_demo, warped_demo, traj):
arms_used = best_demo["arms_used"]
if arms_used in "lb":
pose_array = conversions.array_to_pose_array(
asarray(best_demo["l_gripper_tool_frame"]["position"]),
'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(1, 0, 0, 1),
ns="make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(
asarray(warped_demo["l_gripper_tool_frame"]["position"]),
'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(0, 1, 0, 1),
ns="make_verb_traj_service"))
if arms_used in "rb":
pose_array = conversions.array_to_pose_array(
asarray(best_demo["r_gripper_tool_frame"]["position"]),
'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(1, 0, 0, 1),
ns="make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(
asarray(warped_demo["r_gripper_tool_frame"]["position"]),
'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(0, 1, 0, 1),
ns="make_verb_traj_service"))
Globals.handles.extend(
Globals.rviz.draw_trajectory(traj.l_gripper_poses,
traj.l_gripper_angles,
ns="make_verb_traj_service_grippers"))
if arms_used == 'b':
Globals.handles.extend(
Globals.rviz.draw_trajectory(traj.r_gripper_poses,
traj.r_gripper_angles,
ns="make_verb_traj_service_grippers"))
for (clouds, rgba) in [
(sorted_values(best_demo["object_clouds"]), (1, 0, 0, .5)),
(sorted_values(warped_demo["object_clouds"]), (0, 1, 0, .5))
]:
cloud = []
for subcloud in clouds:
cloud.extend(np.asarray(subcloud["xyz"]).reshape(-1, 3))
cloud = np.array(cloud)
cloud = voxel_downsample(cloud, .02)
pose_array = conversions.array_to_pose_array(cloud, 'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=rgba,
width=.01,
type=Marker.CUBE_LIST))
def default_plot_callback(x_nd, y_md, targ_nd, corr_nm, wt_n, f):
del Globals.handles[:]
plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md)
if targ_nd is not None:
targ_pose_array = conversions.array_to_pose_array(targ_nd, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(targ_pose_array,rgba=(1,1,0,1),type=Marker.CUBE_LIST))
if array_contains(good_xyzs, xyz):
good_inds.append(i)
return good_inds
for object_name in object_names:
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc)).cloud_out
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
xyz = xyz.reshape(-1,3)
rgb = rgb.reshape(-1,3)
if args.do_filtering:
xyz_down = voxel_downsample(xyz, .02)
graph = ri.points_to_graph(xyz_down, .03)
cc = ri.largest_connected_component(graph)
good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()])
good_inds = get_good_inds(xyz, good_xyzs)
good_rgbs = rgb[good_inds]
else:
good_xyzs, good_rgbs = xyz, rgb
outfile.create_group(object_name)
outfile[object_name]["xyz"] = good_xyzs
#outfile[object_name]["rgb"] = good_rgbs
if args.plotting:
rviz = ros_utils.RvizWrapper.create()
rospy.sleep(.5)
from brett2.ros_utils import Marker
pose_array = conversions.array_to_pose_array(good_xyzs.reshape(-1,3), "base_footprint")
plot_handle = rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")
raw_input("press enter when done looking")
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 draw_lines(starts, ends, width, rgba, ns):
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(np.squeeze(cloud), "base_footprint"),
rgba=rgba, type=Marker.CUBE_LIST, ns=ns
))
from brett2 import ros_utils
import geometry_msgs.msg as gm
import rospy
import numpy as np
rospy.init_node("test_draw_gripper")
from jds_utils.conversions import array_to_pose_array
from brett2.ros_utils import RvizWrapper
rviz = RvizWrapper.create()
rospy.sleep(.5)
array = np.array([[0,0,0],[.2,0,0],[.4,0,0]])
pose_array = array_to_pose_array(array, 'base_footprint')
handles = rviz.draw_trajectory(pose_array, [0,.04,.08],'r')
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None):
arms_used = current_stage_info.arms_used
verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name)
if stage_num == 0:
# don't do any extra transformation for the first stage
prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4)
# no special point translation for first stage since no tool yet
special_point_translation = np.identity(4)
elif stage_num > 0:
# make sure that the tool stage only uses one arm (the one with the tool)
assert arms_used in ['r', 'l']
prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name)
prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"]
prev_exp_pc = prev_exp_clouds[0]
prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02)
prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02)
gripper_data_key = "%s_gripper_tool_frame" % (arms_used)
# transform point cloud in base frame to gripper frame
# assume right hand has the tool for now
# use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed
prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1]
prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1]
prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien)
prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform)
prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down])
# get the new point cloud in the new gripper frame
# prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down]
if to_gripper_frame_func is None:
prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key)
else:
prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key)
# get the transformation from the new point cloud to the old point cloud for the previous stage
prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame)
# transforms gripper trajectory point into special point trajectory point
if prev_stage_info.special_point is None:
# if there is no special point, linearize at origin
prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3))
# don't do a special point translation if there is no specified special point
special_point_translation = np.identity(4)
else:
prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point))
# translation from gripper pose in world frame to special point pose in world frame
special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point))
if arms_used != 'b':
arms_used_list = [arms_used]
else:
arms_used_list = ['r', 'l']
warped_stage_data = group_to_dict(verb_stage_data) # deep copy it
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
for arm in arms_used_list:
gripper_data_key = "%s_gripper_tool_frame" % (arm)
# find the special point trajectory before the target transformation
cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"]
cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"]
cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)]
cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats]
# find the transformation from the new special point to the gripper frame
cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation))
# save the demo special point traj for plotting
plot_spec_pt_traj = []
for gripper_mat in cur_demo_gripper_traj_mats:
spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation))
plot_spec_pt_traj.append(spec_pt_xyz)
print 'grip transform:'
print prev_demo_to_exp_grip_transform_lin_rigid
print 'special point translation:'
print special_point_translation
print 'inverse special point translation:'
print cur_exp_inv_special_point_transformation
# find the target transformation for the experiment scene
demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()]
if len(demo_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(demo_object_clouds[0], .02)
y_md = voxel_downsample(cur_exp_clouds[0], .02)
# transformation from old target object to new target object in world frame
cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md)
# apply the target warping transformation to the special point trajectory
cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], []
for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats:
cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat)
cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz)
cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien))
cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens))
plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting
cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)]
# transform the warped special point trajectory back to a gripper trajectory in the experiment
cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats]
warped_stage_data[gripper_data_key]["position"] = []
warped_stage_data[gripper_data_key]["orientation"] = []
for exp_traj_mat in cur_exp_gripper_traj_mats:
warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat)
warped_stage_data[gripper_data_key]["position"].append(warped_pos)
warped_stage_data[gripper_data_key]["orientation"].append(warped_orien)
if arm == 'r':
traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
print "poses: ", len(traj.r_gripper_poses.poses)
traj.r_gripper_angles = warped_stage_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = clouds_frame_id
elif arm == 'l':
traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
print "poses: ", len(traj.l_gripper_poses.poses)
traj.l_gripper_angles = warped_stage_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = clouds_frame_id
Globals.handles = []
plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST))
return resp
def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(
req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [
best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()
]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0], .02)
y_md = voxel_downsample(new_object_clouds[0], .02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd,
y_md,
plotting=2,
reg_init=2,
reg_final=.05,
n_iter=10,
verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented" %
transform_type)
l_offset, r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["l_gripper_tool_frame"]["position"],
warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["r_gripper_tool_frame"]["position"],
warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(0, 0, 1, 1),
width=.01,
type=Marker.CUBE_LIST))
return resp
def plot_curve(points, rgba):
pose_array = conversions.array_to_pose_array(asarray(points), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = rgba,ns = "make_verb_traj_service"))
from brett2 import ros_utils
import geometry_msgs.msg as gm
import rospy
import numpy as np
rospy.init_node("test_draw_gripper")
from jds_utils.conversions import array_to_pose_array
from brett2.ros_utils import RvizWrapper
rviz = RvizWrapper.create()
rospy.sleep(.5)
array = np.array([[0, 0, 0], [.2, 0, 0], [.4, 0, 0]])
pose_array = array_to_pose_array(array, 'base_footprint')
handles = rviz.draw_trajectory(pose_array, [0, .04, .08], 'r')
def rviz_draw_points(pts, **kwargs): pose_array = conversions.array_to_pose_array(np.squeeze(pts), "base_footprint") Globals.handles.append(Globals.rviz.draw_curve(pose_array, **kwargs))