def calc_seg_cost(seg_name, xyz_new_ds, dists_new): candidate_demo = demos[seg_name] xyz_demo_ds = np.squeeze(candidate_demo["cloud_xyz_ds"]) dists_demo = candidate_demo["geodesic_dists"] # cost = recognition.calc_match_score(xyz_new_ds, xyz_demo_ds, dists0 = dists_new, dists1 = dists_demo) cost = recognition.calc_match_score(xyz_demo_ds, xyz_new_ds, dists0 = dists_demo, dists1 = dists_new) print "seg_name: %s. cost: %s"%(seg_name, cost) return cost, seg_name
def calc_seg_cost(seg_name, xyz_new_ds, dists_new): candidate_demo = Globals.demos[seg_name] xyz_demo_ds = np.squeeze(candidate_demo["cloud_xyz_ds"]) dists_demo = candidate_demo["geodesic_dists"] cost = recognition.calc_match_score(xyz_demo_ds, xyz_new_ds, dists0 = dists_demo, dists1 = dists_new) #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) return cost, seg_name
for i0 in xrange(6): rope0 = ropes[i0] n0 = len(rope0) # pairs = zip(xrange(n0), np.random.randint(0,n0,n0)) dists0 = dists[i0] for i1 in xrange(6): rope1 = ropes[i1] dists1 = dists[i1] n1 = len(rope1) print colorize("comparing %s to %s" % (seg_names[i0], seg_names[i1]), "red") cost = recognition.calc_match_score(rope0, rope1, dists0, dists1, plotting=1) results.append((i0, i1, cost)) print i0, i1, cost distmat = np.zeros((6, 6)) for (i0, i1, cost) in results: distmat[i0, i1] = cost distmat[xrange(6), xrange(6)] = np.nan print distmat.argmin(axis=0) a, b = np.meshgrid([0, 3, 1, 4, 2, 5], [0, 3, 1, 4, 2, 5]) distmat_rearr = distmat[a, b] distmat_rearr[range(6), range(6)] = np.nan plt.imshow(distmat_rearr, interpolation="nearest", cmap="gray") plt.show()
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"
for i0 in xrange(6): rope0 = ropes[i0] n0 = len(rope0) #pairs = zip(xrange(n0), np.random.randint(0,n0,n0)) dists0 = dists[i0] for i1 in xrange(6): rope1 = ropes[i1] dists1 = dists[i1] n1 = len(rope1) print colorize("comparing %s to %s" % (seg_names[i0], seg_names[i1]), "red") cost = recognition.calc_match_score(rope0, rope1, dists0, dists1) results.append((i0, i1, cost)) print i0, i1, cost distmat = np.zeros((6, 6)) for (i0, i1, cost) in results: distmat[i0, i1] = cost distmat[xrange(6), xrange(6)] = np.nan print distmat.argmin(axis=0) a, b = np.meshgrid([0, 3, 1, 4, 2, 5], [0, 3, 1, 4, 2, 5]) distmat_rearr = distmat[a, b] distmat_rearr[range(6), range(6)] = np.nan plt.imshow(distmat_rearr, interpolation='nearest', cmap='gray') plt.show()
n0 = len(rope0) #pairs = zip(xrange(n0), np.random.randint(0,n0,n0)) dists0 = dists[i0] for i1 in xrange(6): rope1 = ropes[i1] dists1 = dists[i1] n1 = len(rope1) print colorize("comparing %s to %s" % (seg_names[i0], seg_names[i1]), "red") cost = recognition.calc_match_score(rope0, rope1, dists0, dists1, plotting=1) results.append((i0, i1, cost)) print i0, i1, cost distmat = np.zeros((6, 6)) for (i0, i1, cost) in results: distmat[i0, i1] = cost distmat[xrange(6), xrange(6)] = np.nan print distmat.argmin(axis=0) a, b = np.meshgrid([0, 3, 1, 4, 2, 5], [0, 3, 1, 4, 2, 5]) distmat_rearr = distmat[a, b] distmat_rearr[range(6), range(6)] = np.nan
for i0 in xrange(6): rope0 = ropes[i0] n0 = len(rope0) #pairs = zip(xrange(n0), np.random.randint(0,n0,n0)) dists0 = dists[i0] for i1 in xrange(6): rope1 = ropes[i1] dists1 = dists[i1] n1 = len(rope1) print colorize("comparing %s to %s"%(seg_names[i0], seg_names[i1]), "red") cost = recognition.calc_match_score(rope0, rope1, dists0, dists1) results.append((i0, i1, cost)) print i0, i1, cost distmat = np.zeros((6,6)) for (i0, i1, cost) in results: distmat[i0, i1] = cost distmat[xrange(6),xrange(6)] = np.nan print distmat.argmin(axis=0) a,b = np.meshgrid([0,3,1,4,2,5],[0,3,1,4,2,5]) distmat_rearr = distmat[a,b] distmat_rearr[range(6), range(6)] = np.nan plt.imshow(distmat_rearr,interpolation='nearest',cmap='gray') plt.show()