def calc_match_score(xyz0, xyz1, dists0 = None, dists1 = None, plotting = False): """ calculate similarity between xyz0 and xyz1 using geodesic distances """ f,info = registration.tps_rpm(xyz0, xyz1, plotting=plotting,reg_init=1,reg_final=.1,n_iter=21, verbose=False, return_full=True) partners = info["corr_nm"].argmax(axis=1) starts, ends = np.meshgrid(partners, partners) reses = [.05, .07, .09] nres = len(reses) targ_dist_mats, src_dist_mats = [],[] for i in xrange(nres): dists0 = calc_geodesic_distances(xyz0, reses[i]) dists1 = calc_geodesic_distances(xyz1, reses[i]) dists_targ = dists1[starts, ends] dists0_normed = dists0 / np.median(dists0) dists_targ_normed = dists_targ / np.median(dists1) src_dist_mats.append(dists0_normed) targ_dist_mats.append(dists_targ_normed) distmat = np.empty((nres, nres)) for i in xrange(nres): for j in xrange(nres): distmat[i,j] = np.abs(src_dist_mats[i] - targ_dist_mats[j]).mean() print "dist at res:", distmat, distmat.min() return distmat.min()
def test_icp(): matobj = sio.loadmat(osp.join(osp.dirname(lfd.__file__), "matlab", "pointset_pair.mat")) points0 = matobj["xy1"][:-1] points1 = matobj["xy2"][:-1] tps = registration.tps_rpm(points0, points1, plotting = True)
def test_registration_rope_images(): library = trajectory_library.TrajectoryLibrary("lm.h5","read") plt.ion() segment_group = library.root["segments"] n_segs = len(segment_group.keys()) n_rows = int(round(np.sqrt(n_segs))) n_cols = int(np.ceil(n_segs / n_rows)) print n_rows, n_cols sim_ropes = [] for (i,(name, data)) in enumerate(segment_group.items()): plt.subplot(n_rows, n_cols, i+1) if "object_points" in segment_group: points_n3 = data["object_points"][0] else: print "rope field is deprecated. use object points" points_n3 = data["rope"][0] plt.plot(points_n3[:,1], points_n3[:,0],'.') plt.title(name) sim_ropes.append(points_n3) i=int(sys.argv[1]) rope = np.dot(np.squeeze(np.loadtxt("/home/joschu/Data/rope/rope%i.txt"%i)),np.diag([1,-1,-1])) rope = voxel_downsample(rope,.03) #f = registration.tps_rpm tps = registration.tps_rpm(rope[:,:2], sim_ropes[i][:,:2],plotting=10,reg_init=1,reg_final=.025,n_iter=200)
def match_and_calc_shape_context(xyz_demo_ds, xyz_new_ds, hists_demo=None, hists_new=None, normalize_costs=False, return_tuple=False): def compare_hists(h1, h2): assert h2.shape == h1.shape # (single terms of) chi-squared test statistic return 0.5 / h1.shape[0] * ((h2 - h1)**2 / (h2 + h1 + 1)) f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting=False, reg_init=1, reg_final=.1, n_iter=21, verbose=False) partners = f.corr.argmax(axis=1) if hists_demo is None: hists_demo = calc_shape_context_hists(xyz_demo_ds) if hists_new is None: hists_new = calc_shape_context_hists(xyz_new_ds[partners]) costs = compare_hists(hists_demo, hists_new).sum(axis=1).sum(axis=1) if normalize_costs: print max(costs) m = max(0.5, max(costs)) if m != 0: costs /= m if return_tuple: return f, partners, hists_demo, hists_new, costs return costs
def fit_warp(args, cloud1, cloud2, set_warp_params=None): def read_warp_params(params): return ([float(p.strip()) for p in set_warp_params.strip().split()]) if args.warp_type == 'nonrigid': f = registration.tps_rpm(cloud1, cloud2, plotting=False, reg_init=1, reg_final=.1, n_iter=21, verbose=False) elif args.warp_type == 'rigid3d': f = registration.Rigid3d() if set_warp_params is not None: f.set_params(read_warp_params(set_warp_params)) else: f.fit(cloud1, cloud2) elif args.warp_type == 'translation3d': f = registration.Translation3d() if set_warp_params is not None: f.set_params(read_warp_params(set_warp_params)) else: f.fit(cloud1, cloud2) else: raise NotImplementedError return f
def test_registration_3d(): #if __name__ == "__main__": import rospy, itertools, glob from jds_utils.colorize import colorize if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/Data/rope1" files = sorted(glob.glob(osp.join(data_dir,"*.txt"))) distmat1 = np.zeros((len(files), len(files))) distmat2 = np.zeros((len(files), len(files))) for (i0, i1) in itertools.combinations(xrange(12),2): print colorize("comparing %s to %s"%(files[i0], files[i1]),'red',bold=True) rope0 = np.loadtxt(osp.join(data_dir,files[i0])) rope1 = np.loadtxt(osp.join(data_dir,files[i1])) f = registration.tps_rpm(rope0, rope1, plotting=1,reg_init=100,reg_final=1,n_iter=51, verbose=False) ##distmat1[i0, i1] = f.cost #distmat2[i0, i1] = f.corr_sum break plt.figure(1) plt.imshow(distmat1) plt.title("distances") plt.figure(2) plt.imshow(distmat2) plt.title("corr_sums") np.savez("cross_registration_results", distmat = distmat1, names = files)
def calc_match_score(xyz0, xyz1, dists0 = None, dists1 = None): f = registration.tps_rpm(xyz0, xyz1, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False) corr = f.corr partners = corr.argmax(axis=1) if dists0 is None: dists0 = calc_geodesic_distances(xyz0) if dists1 is None: dists1 = calc_geodesic_distances(xyz1) starts, ends = np.meshgrid(partners, partners) dists_targ = dists1[starts, ends] return np.abs(dists0 - dists_targ).sum()/dists0.size
def get_warping_transform(from_cloud, to_cloud, transform_type="tps"): if transform_type == "tps": return registration.tps_rpm(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False) elif transform_type == "tps_zrot": return registration.tps_rpm_zrot(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False) elif transform_type == "translation2d": warp = registration.Translation2d() warp.fit(from_cloud, to_cloud) return warp elif transform_type == "rigid2d": warp = registration.Rigid2d() warp.fit(from_cloud, to_cloud) return warp else: raise Exception("transform type %s is not yet implemented" % transform_type)
def test_multi_fitting(): library = trajectory_library.TrajectoryLibrary(osp.join(osp.dirname(lfd.__file__),"data/lm.h5"),"read") drawn_points = np.load(osp.join(osp.dirname(lfd.__file__),"data/should_be_demo01.txt.npy")) best_cost = np.inf best_f = None best_name = None for (seg_name,trajectory) in library.root["segments"].items(): #f = registration.ThinPlateSpline() #f.fit(trajectory["rope"][0], #drawn_points, 1e-2,1e-2) f = registration.tps_rpm(np.asarray(trajectory["rope"][0][::-1]), drawn_points, plotting = False) print "seg_name: %s. cost: %s"%(seg_name, f.cost) if f.cost < best_cost: best_cost = f.cost best_f = f best_name = seg_name seg_name = best_name print seg_name
def test_cups(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_image_proc.pcd_io import load_xyzrgb if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/Data/cups" xyz1, rgb1 = load_xyzrgb(osp.join(data_dir,"cups1.pcd")) def preproc(xyz1): xyz1 = xyz1.reshape(-1,3) xyz1 = np.dot(xyz1, np.diag([1,-1,-1])) xyz1 = xyz1[(xyz1[:,2] > .02) & (xyz1[:,2] < .2) & (np.abs(xyz1[:,0]) < .15) & (np.abs(xyz1[:,1]) < .3)] xyz1 = voxel_downsample(xyz1, .015) return xyz1 xyz1 = preproc(xyz1) #from mayavi import mlab #mlab.points3d(*xyz1.T,color=(1,1,1),scale_factor=.01) xyz2, rgb2 = load_xyzrgb(osp.join(data_dir,"cups4.pcd")) xyz2 = preproc(xyz2) f = registration.tps_rpm(3*xyz1, 3*xyz2, plotting=4,reg_init=1,reg_final=.05,n_iter=200, verbose=False)
def fit_warp(args, cloud1, cloud2, set_warp_params=None): def read_warp_params(params): return ([float(p.strip()) for p in set_warp_params.strip().split()]) if args.warp_type == 'nonrigid': f = registration.tps_rpm(cloud1, cloud2, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False) elif args.warp_type == 'rigid3d': f = registration.Rigid3d() if set_warp_params is not None: f.set_params(read_warp_params(set_warp_params)) else: f.fit(cloud1, cloud2) elif args.warp_type == 'translation3d': f = registration.Translation3d() if set_warp_params is not None: f.set_params(read_warp_params(set_warp_params)) else: f.fit(cloud1, cloud2) else: raise NotImplementedError return f
def match_and_calc_shape_context(xyz_demo_ds, xyz_new_ds, hists_demo=None, hists_new=None, normalize_costs=False, return_tuple=False): def compare_hists(h1, h2): assert h2.shape == h1.shape # (single terms of) chi-squared test statistic return 0.5/h1.shape[0] * ((h2 - h1)**2 / (h2 + h1 + 1)) f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False) partners = f.corr.argmax(axis=1) if hists_demo is None: hists_demo = calc_shape_context_hists(xyz_demo_ds) if hists_new is None: hists_new = calc_shape_context_hists(xyz_new_ds[partners]) costs = compare_hists(hists_demo, hists_new).sum(axis=1).sum(axis=1) if normalize_costs: print max(costs) m = max(0.5, max(costs)) if m != 0: costs /= m if return_tuple: return f, partners, hists_demo, hists_new, costs return costs
plt.imshow(distmat1) plt.title("distances") plt.figure(2) plt.imshow(distmat2) plt.title("corr_sums") np.savez("cross_registration_results", distmat = distmat1, names = files) if __name__ == "__main__": import rospy, itertools, glob from utils.colorize import colorize from image_proc.pcd_io import load_xyzrgb if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/Data/cups" xyz1, rgb1 = load_xyzrgb(osp.join(data_dir,"cups1.pcd")) def preproc(xyz1): xyz1 = xyz1.reshape(-1,3) xyz1 = np.dot(xyz1, np.diag([1,-1,-1])) xyz1 = xyz1[(xyz1[:,2] > .02) & (xyz1[:,2] < .2) & (np.abs(xyz1[:,0]) < .15) & (np.abs(xyz1[:,1]) < .3)] xyz1 = voxel_downsample(xyz1, .015) return xyz1 xyz1 = preproc(xyz1) #from mayavi import mlab #mlab.points3d(*xyz1.T,color=(1,1,1),scale_factor=.01) xyz2, rgb2 = load_xyzrgb(osp.join(data_dir,"cups4.pcd")) xyz2 = preproc(xyz2) f = registration.tps_rpm(3*xyz1, 3*xyz2, plotting=4,reg_init=1,reg_final=.05,n_iter=200, verbose=False)
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"
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"
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 get_tps_transform(from_cloud, to_cloud): #return registration.tps_rpm(from_cloud, to_cloud) return registration.tps_rpm(from_cloud, to_cloud, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
def callback(req): assert isinstance(req, MakeTrajectoryRequest) if req.verb not in h5file: rospy.logerr("verb %s was not found in library",req.verb) demo_group = h5file[req.verb]["demos"] new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] rospy.logwarn("using first demo") best_demo_name = demo_group.keys()[0] best_demo = demo_group[best_demo_name] assert "transform" in verb_info[req.verb] transform_type = verb_info[req.verb]["transform"] x_nd = voxel_downsample(asarray(best_demo["object_clouds"].values()[0]),.02) y_md = voxel_downsample(new_object_clouds[0],.02) if transform_type == "tps": warp = registration.tps_rpm(x_nd, y_md, plotting = 4, reg_init = 2, reg_final = .1, n_iter = 39) 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) this_verb_info = verb_info[req.verb] l_offset,r_offset = np.zeros(3), np.zeros(3) if "r_tool" in this_verb_info: r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) if "l_tool" in this_verb_info: l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) if "r_offset" in this_verb_info: r_offset += asarray(this_verb_info["r_offset"]) if "l_offset" in this_verb_info: l_offset += asarray(this_verb_info["l_offset"]) arms_used = best_demo["arms_used"].value warped_demo = warping.transform_demo(warp, best_demo,arms_used in 'lb', arms_used in 'rb',object_clouds=True, l_offset = l_offset, r_offset = r_offset) resp = MakeTrajectoryResponse() traj = resp.traj if arms_used == "l": traj.arms_used = "l" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"]) traj.gripper0_angles = warped_demo["l_gripper_angles"] if "l_tool" in this_verb_info: traj.gripper0_angles *= 0 elif arms_used == "r": traj.arms_used = "r" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"]) traj.gripper0_angles = warped_demo["r_gripper_angles"] if "r_tool" in this_verb_info: traj.gripper0_angles *= 0 elif arms_used == "b": traj.arms_used = "b" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"]) traj.gripper1_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"]) traj.gripper0_angles = warped_demo["l_gripper_angles"] traj.gripper1_angles = warped_demo["r_gripper_angles"] if "l_tool" in this_verb_info: traj.gripper0_angles *= 0 if "r_tool" in this_verb_info: traj.gripper1_angles *= 0 traj.gripper0_poses.header.frame_id = req.object_clouds[0].header.frame_id traj.gripper1_poses.header.frame_id = req.object_clouds[0].header.frame_id Globals.handles = [] plot_original_and_warped_demo(best_demo, warped_demo, traj) 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) 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"
if __name__ == "__main__": import rospy, itertools, glob from jds_utils.colorize import colorize if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/Data/rope1" files = sorted(glob.glob(osp.join(data_dir,"*.txt"))) distmat1 = np.zeros((len(files), len(files))) distmat2 = np.zeros((len(files), len(files))) for (i0, i1) in itertools.combinations(xrange(12),2): print colorize("comparing %s to %s"%(files[i0], files[i1]),'red',bold=True) rope0 = np.loadtxt(osp.join(data_dir,files[i0])) rope1 = np.loadtxt(osp.join(data_dir,files[i1])) f = registration.tps_rpm(rope0, rope1, plotting=1,reg_init=100,reg_final=1,n_iter=51, verbose=False) ##distmat1[i0, i1] = f.cost #distmat2[i0, i1] = f.corr_sum break plt.figure(1) plt.imshow(distmat1) plt.title("distances") plt.figure(2) plt.imshow(distmat2) plt.title("corr_sums") np.savez("cross_registration_results", distmat = distmat1, names = files) #if __name__ == "__main__": def test_cups():
dists = ssd.cdist(np.array([z0src, z1src]), x) wts = np.exp(-dists/.01) wts = wts / wts.sum(axis=0)[None,:] return wts[0][:,None] * local_trans(x, z0src, z0tgt, theta0) + \ wts[1][:,None] * local_trans(x, z1src, z1tgt, theta1) soln = so.fmin_cg(cost, np.r_[z0tgt, 0, z1tgt, 0],callback=plotter.callback) def warp_fn(x, params): s_z0tgt = params[:2] s_theta0 = params[2] s_z1tgt = params[3:5] s_theta1 = params[5] return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1) plt.figure(2) tps_rpm(uvs_src, uvs_tgt, n_iter = 100, reg_init=1, reg_final = .1, plotting=4, rad_init=.01, rad_final = .001, f_init = lambda x: warp_fn(x, soln)) #plt.plot(mug0src[:,1], mug0src[:,0],'cx') #plt.plot(mug0tgt[:,1], mug0tgt[:,0],'cx') show()
wts = wts / wts.sum(axis=0)[None, :] return wts[0][:,None] * local_trans(x, z0src, z0tgt, theta0) + \ wts[1][:,None] * local_trans(x, z1src, z1tgt, theta1) soln = so.fmin_cg(cost, np.r_[z0tgt, 0, z1tgt, 0], callback=plotter.callback) def warp_fn(x, params): s_z0tgt = params[:2] s_theta0 = params[2] s_z1tgt = params[3:5] s_theta1 = params[5] return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1) plt.figure(2) tps_rpm(uvs_src, uvs_tgt, n_iter=100, reg_init=1, reg_final=.1, plotting=4, rad_init=.01, rad_final=.001, f_init=lambda x: warp_fn(x, soln)) #plt.plot(mug0src[:,1], mug0src[:,0],'cx') #plt.plot(mug0tgt[:,1], mug0tgt[:,0],'cx') show()
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 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}