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 plot_orig_and_warped_clouds(f, x_nd, y_md, res=.1, d=3): if d == 2: import matplotlib.pyplot as plt plt.plot(x_nd[:, 1], x_nd[:, 0], 'r.') plt.plot(y_md[:, 1], y_md[:, 0], 'b.') pred = f(x_nd) if d == 2: plt.plot(pred[:, 1], pred[:, 0], 'g.') if d == 2: plot_warped_grid_2d(f, x_nd.min(axis=0), x_nd.max(axis=0)) plt.ginput() elif d == 3: 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.extend( warping.draw_grid(Globals.rviz, f, mins, maxes, 'base_footprint', xres=res, yres=res, zres=-1)) draw_orig_new_warped_pcs(x_nd, y_md, f(x_nd))
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): 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) 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) # 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=.2) wt_n = corr_nm.sum(axis=1) targ_nd = np.dot(corr_nm/wt_n[:,None], y_md) # if plotting: # plot_correspondence(x_nd, targ_nd) f.fit(x_nd, targ_nd, regs[i], wt_n = wt_n, angular_spring = regs[i]*200, verbose=verbose) if plotting and i%plotting==0: if f.d==2: plt.plot(x_nd[:,1], x_nd[:,0],'r.') plt.plot(y_md[:,1], y_md[:,0], 'b.') pred = f.transform_points(x_nd) if f.d==2: plt.plot(pred[:,1], pred[:,0], 'g.') if f.d == 2: plot_warped_grid_2d(f.transform_points, x_nd.min(axis=0), x_nd.max(axis=0)) plt.ginput() elif f.d == 3: from lfd import warping from brett2.ros_utils import Marker from utils import conversions Globals.setup() mins = x_nd.min(axis=0) maxes = x_nd.max(axis=0) mins[2] -= .1 maxes[2] += .1 Globals.handles = warping.draw_grid(Globals.rviz, f.transform_points, mins, maxes, 'base_footprint', xres=.1, yres=.1) 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.transform_points(x_nd), 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST)) Globals.handles.append(Globals.rviz.draw_curve(target_pose_array,rgba=(0,0,1,1),type=Marker.CUBE_LIST)) Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),type=Marker.CUBE_LIST)) f.corr = corr_nm return f
def plot_orig_and_warped_clouds(f, x_nd, y_md, res=.1, d=3): if d==2: import matplotlib.pyplot as plt plt.plot(x_nd[:,1], x_nd[:,0],'r.') plt.plot(y_md[:,1], y_md[:,0], 'b.') pred = f(x_nd) if d==2: plt.plot(pred[:,1], pred[:,0], 'g.') if d == 2: plot_warped_grid_2d(f, x_nd.min(axis=0), x_nd.max(axis=0)) plt.ginput() elif d == 3: 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.extend(warping.draw_grid(Globals.rviz, f, mins, maxes, 'base_footprint', xres=res, yres=res, zres=-1)) draw_orig_new_warped_pcs(x_nd, y_md, f(x_nd))
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 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 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}
def callback(request): Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') #Globals.rviz.remove_all_markers() #draw_table() new_cloud1, _ = pc2xyzrgb(request.object_clouds[0]) new_cloud2, _ = pc2xyzrgb(request.object_clouds[1]) new_cloud1, new_cloud2 = sorted([new_cloud1, new_cloud2], key = lambda x: np.squeeze(x).ptp(axis=0).prod()) new_cloud1 = new_cloud1.reshape(-1,3) new_cloud2 = new_cloud2.reshape(-1,3) new_xyz1 = (new_cloud1.max(axis=0) + new_cloud1.min(axis=0))/2 new_xyz2 = (new_cloud2.max(axis=0) + new_cloud2.min(axis=0))/2 f.fit(np.array([xyz1, xyz2]), np.array([new_xyz1, new_xyz2]), 1e6, 1e-3) new_gripper_xyzs, new_gripper_mats = f.transform_frames(old_gripper_xyzs, conversions.quats2mats(old_gripper_quats)) new_gripper_quats = conversions.mats2quats(new_gripper_mats) #print "warning: using old oreitnations" show_objects_and_trajectory(new_gripper_xyzs, np.array([new_xyz1, new_xyz2]), np.array([quat1, quat2]), obj_dims, (0,1,0,1)) show_objects_and_trajectory(old_gripper_xyzs, np.array([xyz1, xyz2]), np.array([quat1, quat2]), obj_dims, (0,0,1,1)) grid_handle = draw_grid(Globals.rviz, f.transform_points, old_gripper_xyzs.min(axis=0), old_gripper_xyzs.max(axis=0), "base_footprint") HANDLES.append(grid_handle) Globals.pr2.join_all() Globals.pr2.update_rave() best_traj_info, best_feasible_frac = None, 0 env = Globals.pr2.robot.GetEnv() Globals.pr2.update_rave() collision_env = create_obstacle_env(env) basemanip = openravepy.interfaces.BaseManipulation(collision_env.GetRobot("pr2"),plannername=None) rospy.sleep(.1) #collision_env.SetViewer("qtcoin") #raw_input("press enter to continue") for (lr, arm) in zip("lr",[Globals.pr2.larm,Globals.pr2.rarm]): name = arm.manip.GetName() manip = collision_env.GetRobot('pr2').GetManipulator(name) rospy.loginfo("trying to plan to initial position with %s",name) first_mat1 = np.eye(4) first_mat1[:3,:3] = new_gripper_mats[0] first_mat1[:3,3] = new_gripper_xyzs[0] first_mat = transform_relative_pose_for_ik(manip, first_mat1, "world", "%s_gripper_tool_frame"%lr) collision_env.GetRobot("pr2").SetActiveManipulator(name) trajobj = None try: trajobj = basemanip.MoveToHandPosition([first_mat],seedik=16,outputtrajobj=True,execute=0) rospy.loginfo("planning succeeded") except Exception: rospy.loginfo("planning failed") traceback.print_exc() print "initial ik result", manip.FindIKSolutions(first_mat,0) continue rospy.loginfo("trying ik") Globals.pr2.update_rave() joint_positions, inds = trajectories.make_joint_traj(new_gripper_xyzs, new_gripper_quats, manip, 'base_footprint', '%s_gripper_tool_frame'%lr, filter_options = 1+16) feasible_frac = len(inds)/len(new_gripper_xyzs) print inds if feasible_frac > best_feasible_frac: best_feasible_frac = feasible_frac best_traj_info = dict( feasible_frac = feasible_frac, lr = 'l' if name == 'leftarm' else 'r', manip = manip, arm = arm, initial_traj = trajobj, joint_positions = joint_positions) collision_env.Destroy() response = PourResponse() if best_feasible_frac > .75: if best_traj_info["initial_traj"] is not None: follow_rave_traj(best_traj_info["arm"], best_traj_info["initial_traj"]) else: print "no initial traj" #print "feasible inds", best_traj_info["inds"] body_traj = np.zeros(len(best_traj_info["joint_positions"]),dtype=trajectories.BodyState) lr = best_traj_info["lr"] body_traj["%s_arm"%lr] = best_traj_info["joint_positions"] body_traj["%s_gripper"%lr] = gripper_angles trajectories.follow_body_traj(Globals.pr2, body_traj, times, r_arm = (lr=='r'), r_gripper = (lr=='r'), l_arm = (lr=='l'), l_gripper= (lr=='l')) Globals.pr2.join_all() response.success = True else: rospy.logerr("could not execute trajectory because not enough points are reachable") response.success = False return response