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()
